public static CarModelState ToCarModelState(GridCarModelState s) { CarModelState cms = new CarModelState(); cms.Position = new PointD(Math.Cos(s.TargetAngle - s.TargetFinishAngle) * s.TargetDist, Math.Sin(s.TargetAngle - s.TargetFinishAngle) * s.TargetDist); cms.Angle = Math.PI - s.TargetFinishAngle; return cms; }
public void newTrainEpoch() { lock (carModelGraphicControl1.trainingModels) { carModelGraphicControl1.trainingModels = new List <CarModel>(); lock (carModelGraphicControl1.trainingModels) { if (neuralController.trainInnerStates != null) { foreach (GridCarModelState s in neuralController.trainInnerStates) { carModelGraphicControl1.trainingModels.Add(new CarModel(GridCarModelState.ToCarModelState(s))); } } if (neuralController.trainInnerStatesOrig != null) { foreach (GridCarModelState s in neuralController.trainInnerStatesOrig) { carModelGraphicControl1.trainingModels.Add(new CarModel(GridCarModelState.ToCarModelState(s), Color.Brown)); } } } } }
public static CarModelState ToCarModelState(GridCarModelState s) { CarModelState cms = new CarModelState(); cms.Position = new PointD(Math.Cos(s.TargetAngle - s.TargetFinishAngle) * s.TargetDist, Math.Sin(s.TargetAngle - s.TargetFinishAngle) * s.TargetDist); cms.Angle = Math.PI - s.TargetFinishAngle; return(cms); }
public static GridCarModelState FromCarModelState(CarModelState s) { GridCarModelState gcms = new GridCarModelState(); gcms.TargetFinishAngle = Math.PI-s.Angle; gcms.TargetAngle = Math.Atan2(s.Position.Y, s.Position.X) + gcms.TargetFinishAngle; gcms.TargetDist = Math.Sqrt(s.Position.Y * s.Position.Y + s.Position.X * s.Position.X); return gcms; }
public static GridCarModelState FromCarModelState(CarModelState s) { GridCarModelState gcms = new GridCarModelState(); gcms.TargetFinishAngle = Math.PI - s.Angle; gcms.TargetAngle = Math.Atan2(s.Position.Y, s.Position.X) + gcms.TargetFinishAngle; gcms.TargetDist = Math.Sqrt(s.Position.Y * s.Position.Y + s.Position.X * s.Position.X); return(gcms); }
public void Simulate(GridCarModelState initialState, int simCount, out GridCarModelInput[] inputs, out GridCarModelState[] states) { inputs = new GridCarModelInput[simCount]; states = new GridCarModelState[simCount]; GridCarModelState state = initialState; for (int i = 0; i < simCount; ++i) { SimulateOneStep(state, out inputs[i], out states[i]); state = states[i]; } }
//private double obstacleFieldError(GridCarModelState state) //{ // double err = 0; // List<ObstacleState> obstacles = obstacleProvider.GetObstacleStates(1); // foreach (ObstacleState obst in obstacles) // { // double d = ComMath.Normal(state.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, MIN_NEURON_VALUE, MAX_NEURON_VALUE); // double x = Math.Cos(state.TargetAngle - state.TargetFinishAngle) * d; // double y = Math.Sin(state.TargetAngle - state.TargetFinishAngle) * d; // double x0 = ComMath.Normal(obst.pp.position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE); // double y0 = ComMath.Normal(obst.pp.position.Y, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE); // double dist = Math.Sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0)); // err += Math.Pow(1 / dist - 1 / obst.radius, 2); // } // return err; //} private static PolarGrid obstaclePolarGrid(IObstaclePositionProvider ops, GridCarModelState state) { List <ObstacleState> obstacles = ops.GetObstacleStates(0); PolarGrid pg = new PolarGrid(); foreach (ObstacleState obst in obstacles) { GridObstacleState gos = GridObstacleState.FromObstacleState(obst, state); pg.AddObstacle(gos); } return(pg); }
private static double[] obstacleFieldErrorGradient(IObstaclePositionProvider ops, GridCarModelState state, int time) { //C = sum((1/d(X) - 1/d(0))^2) //dC/dy_x =... //dC/dy_y =... double ksi = 0.1; double disterr = 0; double orxerr = 0; double oryerr = 0; List<ObstacleState> obstacles = ops.GetObstacleStates(0); foreach (ObstacleState obst in obstacles)//cel az origo, tehat az origohoz relativak az akadalyok, origo felfele nez { double d = ComMath.Normal(Math.Sqrt(obst.pp.position.X * obst.pp.position.X + obst.pp.position.Y * obst.pp.position.Y), GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, 0, MAX_NEURON_VALUE); double a = ComMath.Normal(state.TargetDist , GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, 0, MAX_NEURON_VALUE); double ang = Math.PI - (Math.Atan2(obst.pp.position.Y, obst.pp.position.X) + Math.PI) + state.TargetAngle - state.TargetFinishAngle; if (ang > Math.PI) ang -= 2 * Math.PI; if (ang < -Math.PI) ang += 2 * Math.PI; double AA = -2 * d * Math.Cos(ang); double BB = d * d; double obstdist = Math.Sqrt(a * a + BB + AA * a); double obstang = state.TargetAngle + Math.Sign(ang) * Math.Acos((a * a + obstdist * obstdist - d * d) / (2 * a * obstdist)); double r = ComMath.Normal(obst.radius + CarModel.SHAFT_LENGTH / 2, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, 0, MAX_NEURON_VALUE); double dist = obstdist - r; if (dist <= 0.0001) dist = 0.0001; double err = 1 / (2 * (dist * dist * dist)); disterr += -(AA + 2 * a) * err; double angerr = (-2 * a * d * Math.Sin(ang)) * err; orxerr += -Math.Sin(state.TargetAngle) * angerr; oryerr += Math.Cos(state.TargetAngle) * angerr; } if (obstacles.Count > 0) { disterr /= obstacles.Count; orxerr /= obstacles.Count; oryerr /= obstacles.Count; } return new double[] { -ksi * disterr, -ksi * orxerr, -ksi * oryerr }; }
public static GridObstacleState FromObstacleState(ObstacleState obst, GridCarModelState state) { double d = ComMath.Normal(Math.Sqrt(obst.pp.position.X * obst.pp.position.X + obst.pp.position.Y * obst.pp.position.Y), GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, 0, 1); double a = ComMath.Normal(state.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, 0, 1); double ang = Math.PI - (Math.Atan2(obst.pp.position.Y, obst.pp.position.X) + Math.PI) + state.TargetAngle - state.TargetFinishAngle; if (ang > Math.PI) ang -= 2 * Math.PI; if (ang < -Math.PI) ang += 2 * Math.PI; double AA = -2 * d * Math.Cos(ang); double BB = d * d; double obstdist = Math.Sqrt(a * a + BB + AA * a); double obstang = state.TargetAngle + Math.Sign(ang) * Math.Acos((a * a + obstdist * obstdist - d * d) / (2 * a * obstdist)); GridObstacleState gos = new GridObstacleState(obstdist, obstang, obst.radius); return gos; }
public void SimulateModel(GridCarModelState state, GridCarModelInput input, double timeStep, out GridCarModelState output) { this.state = state; this.input = input; GridCarModelState state2 = state; double dAngle = (input.LeftSpeed - input.RightSpeed) * timeStep / CarModel.SHAFT_LENGTH; double lamda = 1; if (dAngle != 0) lamda = 2 / dAngle * Math.Sin(dAngle / 2); double vectLength = (input.RightSpeed + input.LeftSpeed) / 2 * timeStep * lamda; state2.TargetDist = Math.Sqrt(state.TargetDist * state.TargetDist + vectLength * vectLength - 2 * state.TargetDist * vectLength * Math.Cos(state.TargetAngle - dAngle / 2)); state2.TargetAngle = state.TargetAngle - dAngle + Math.Sign(state.TargetAngle - dAngle / 2) * Math.Acos((state.TargetDist * state.TargetDist + state2.TargetDist * state2.TargetDist - vectLength * vectLength) / (2 * state.TargetDist * state2.TargetDist)); state2.TargetFinishAngle -= dAngle; output = state2; }
public void SimulateModel(GridCarModelState state, GridCarModelInput input, double timeStep, out GridCarModelState output) { this.state = state; this.input = input; GridCarModelState state2 = state; double dAngle = (input.LeftSpeed - input.RightSpeed) * timeStep / CarModel.SHAFT_LENGTH; double lamda = 1; if (dAngle != 0) { lamda = 2 / dAngle * Math.Sin(dAngle / 2); } double vectLength = (input.RightSpeed + input.LeftSpeed) / 2 * timeStep * lamda; state2.TargetDist = Math.Sqrt(state.TargetDist * state.TargetDist + vectLength * vectLength - 2 * state.TargetDist * vectLength * Math.Cos(state.TargetAngle - dAngle / 2)); state2.TargetAngle = state.TargetAngle - dAngle + Math.Sign(state.TargetAngle - dAngle / 2) * Math.Acos((state.TargetDist * state.TargetDist + state2.TargetDist * state2.TargetDist - vectLength * vectLength) / (2 * state.TargetDist * state2.TargetDist)); state2.TargetFinishAngle -= dAngle; output = state2; }
public void CalcErrorSensibility(double[] errors, out double[] sensitibility)//errors: distance error, orientation x,y error, finish orientation x,y error { GridCarModelState output1, output2, state1, state2, origiState = this.state; GridCarModelInput input1, input2, origiInput = this.input; double DIFF_C = 0.001; sensitibility = new double[6]; //****** //DIST //****** state1 = origiState; state1.TargetDist = ComMath.Normal(ComMath.Normal(state1.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST); this.SimulateModel(state1, origiInput, out output1); state2 = origiState; state2.TargetDist = ComMath.Normal(ComMath.Normal(state2.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST); this.SimulateModel(state2, origiInput, out output2); sensitibility[0] = (ComMath.Normal(output2.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] + (ComMath.Normal(output2.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] + (ComMath.Normal(output2.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] + (ComMath.Normal(output2.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3] + (ComMath.Normal(output2.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[4]; //****** //ORIENTATION X //****** state1 = origiState; state1.TargetOrientation = new PointD(ComMath.Normal(ComMath.Normal(state1.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY), state1.TargetOrientation.Y); this.SimulateModel(state1, origiInput, out output1); state2 = origiState; state2.TargetOrientation = new PointD(ComMath.Normal(ComMath.Normal(state2.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY), state2.TargetOrientation.Y); this.SimulateModel(state2, origiInput, out output2); sensitibility[1] = (ComMath.Normal(output2.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] + (ComMath.Normal(output2.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] + (ComMath.Normal(output2.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] + (ComMath.Normal(output2.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3] + (ComMath.Normal(output2.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[4]; //****** //ORIENTATION Y //****** state1 = origiState; state1.TargetOrientation = new PointD(state1.TargetOrientation.X, ComMath.Normal(ComMath.Normal(state1.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY)); this.SimulateModel(state1, origiInput, out output1); state2 = origiState; state2.TargetOrientation = new PointD(state2.TargetOrientation.X, ComMath.Normal(ComMath.Normal(state2.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY)); this.SimulateModel(state2, origiInput, out output2); sensitibility[2] = (ComMath.Normal(output2.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] + (ComMath.Normal(output2.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] + (ComMath.Normal(output2.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] + (ComMath.Normal(output2.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3] + (ComMath.Normal(output2.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[4]; //****** //FINISH ORIENTATION X //****** state1 = origiState; state1.TargetFinishOrientation = new PointD(ComMath.Normal(ComMath.Normal(state1.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY), state1.TargetFinishOrientation.Y); this.SimulateModel(state1, origiInput, out output1); state2 = origiState; state2.TargetFinishOrientation = new PointD(ComMath.Normal(ComMath.Normal(state2.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY), state2.TargetFinishOrientation.Y); this.SimulateModel(state2, origiInput, out output2); sensitibility[3] = (ComMath.Normal(output2.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] + (ComMath.Normal(output2.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] + (ComMath.Normal(output2.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] + (ComMath.Normal(output2.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3] + (ComMath.Normal(output2.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[4]; //****** //FINISH ORIENTATION Y //****** state1 = origiState; state1.TargetFinishOrientation = new PointD(state1.TargetFinishOrientation.X, ComMath.Normal(ComMath.Normal(state1.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY)); this.SimulateModel(state1, origiInput, out output1); state2 = origiState; state2.TargetFinishOrientation = new PointD(state2.TargetFinishOrientation.X, ComMath.Normal(ComMath.Normal(state2.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY)); this.SimulateModel(state2, origiInput, out output2); sensitibility[4] = (ComMath.Normal(output2.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] + (ComMath.Normal(output2.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] + (ComMath.Normal(output2.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] + (ComMath.Normal(output2.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3] + (ComMath.Normal(output2.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[4]; //****** //ANGLE //****** input1 = origiInput; input1.Angle = ComMath.Normal(ComMath.Normal(input1.Angle, GridCarModelInput.MIN_ANGLE, GridCarModelInput.MAX_ANGLE, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, GridCarModelInput.MIN_ANGLE, GridCarModelInput.MAX_ANGLE); this.SimulateModel(origiState, input1, out output1); input2 = origiInput; input2.Angle = ComMath.Normal(ComMath.Normal(input2.Angle, GridCarModelInput.MIN_ANGLE, GridCarModelInput.MAX_ANGLE, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, GridCarModelInput.MIN_ANGLE, GridCarModelInput.MAX_ANGLE); this.SimulateModel(origiState, input2, out output2); sensitibility[5] = (ComMath.Normal(output2.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] + (ComMath.Normal(output2.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] + (ComMath.Normal(output2.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] + (ComMath.Normal(output2.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3] + (ComMath.Normal(output2.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[4]; this.state = origiState; this.input = origiInput; }
//matematikai modell public void SimulateModel(GridCarModelState state, GridCarModelInput input, out GridCarModelState output) { SimulateModel(state, input, CarModel.SIMULATION_TIME_STEP, out output); }
private static double TrainOneEpoch(MLPDll controller, IGridModelSimulator model, ICarPositionProvider cps, IFinishPositionProvider fps, IObstaclePositionProvider ops, double mu, int maxSimCount, out double SumSimCount, out List <GridCarModelState> innerStates) { double sumSimCount = 0; double error = 0; innerStates = new List <GridCarModelState>(); List <double> deltaws = new List <double>(); MLPDll[] controllers = new MLPDll[maxSimCount]; IGridModelSimulator[] models = new IGridModelSimulator[maxSimCount]; GridCarModelState state = GridCarModelState.FromCarModelState(cps.GetCarState()); GridCarModelInput input = new GridCarModelInput(); //kimenet kiszamitasa int simCount = 0; List <double[]> singleErrors = new List <double[]>(); List <double[]> regularizationErrors = new List <double[]>(); GridCarModelState laststate; bool earlyStop; do { if (simCount == 0) { controllers[simCount] = new MLPDll(controller); //lemasoljuk } else { controllers[simCount] = new MLPDll(controllers[simCount - 1]); } models[simCount] = model.Clone();//a modellt is laststate = state; GridNeuralController.SimulateOneStep(controllers[simCount], models[simCount], state, out input, out state);//vegigszimulaljuk a simCount darab controlleren es modellen innerStates.Add(state); //kozbulso hibak kiszamitasa, itt csak az akadalyoktol valo tavolsag "hibajat" vesszuk figyelembe, irany nem szamit -> hibaja 0 regularizationErrors.Add(obstacleFieldErrorGradient(ops, state, simCount)); //minden pont celtol vett tavolsaga double disterror = ComMath.Normal(state.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, 0, 1); double orientationerror = disterror; if (orientationerror < 0.2) { orientationerror = 0; } double finishorientationerror = disterror; if (finishorientationerror > 0.05) { finishorientationerror = 0; } else { finishorientationerror = 1; } double finishX = Math.Cos(Math.PI - fps.GetFinishState(simCount).Angle); double finishY = Math.Sin(Math.PI - fps.GetFinishState(simCount).Angle); singleErrors.Add(new double[] { -disterror * MAX_NEURON_VALUE, orientationerror * ComMath.Normal(1 - state.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE), orientationerror * ComMath.Normal(0 - state.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE), finishorientationerror * ComMath.Normal(finishX - state.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE), finishorientationerror * ComMath.Normal(finishY - state.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE) } ); ++simCount; earlyStop = false; if (simCount > 3) { double[] err1 = singleErrors[simCount - 1]; double[] err2 = singleErrors[simCount - 2]; double[] err3 = singleErrors[simCount - 3]; double error1, error2, error3; error1 = error2 = error3 = 0; for (int i = 0; i < 1; i++)//err1.Length { error1 += err1[i] * err1[i]; error2 += err2[i] * err2[i]; error3 += err3[i] * err3[i]; } earlyStop = ((error1 > error2) && (error3 > error2)); if (earlyStop) { //utolso elemet toroljuk singleErrors.RemoveAt(singleErrors.Count - 1); regularizationErrors.RemoveAt(regularizationErrors.Count - 1); innerStates.RemoveAt(innerStates.Count - 1); --simCount; } } }while ((simCount < maxSimCount) && !earlyStop); double[] errors = singleErrors[singleErrors.Count - 1]; sumSimCount += simCount; //hibavisszaterjesztes for (int i = simCount - 1; i >= 0; --i) { double[] sensitibility; models[i].CalcErrorSensibility(errors, out sensitibility); double[] inputSensitibility; inputSensitibility = new double[1]; inputSensitibility[0] = sensitibility[5]; double[] sensitibility2; controllers[i].SetOutputError(inputSensitibility); controllers[i].Backpropagate(); controllers[i].CalculateDeltaWeights(); sensitibility2 = controllers[i].SensitibilityD(); errors[0] = (sensitibility[0] + sensitibility2[0] + 0.1 * singleErrors[i][0]); errors[1] = (sensitibility[1] + sensitibility2[1] + 0 * singleErrors[i][1]); errors[2] = (sensitibility[2] + sensitibility2[2] + 0 * singleErrors[i][2]); errors[3] = (sensitibility[3] + sensitibility2[3] + singleErrors[i][3]); errors[4] = (sensitibility[4] + sensitibility2[4] + singleErrors[i][4]); //regularizaciobol szarmazo hiba hozzaadasa errors[0] += regularizationErrors[i][0]; errors[1] += regularizationErrors[i][1]; errors[2] += regularizationErrors[i][2]; } controller.ClearDeltaWeights(); //sulymodositasok osszegzese for (int i2 = 0; i2 < simCount; ++i2) { controller.AddDeltaWeights(controllers[i2]); } float maxdw = controller.MaxDeltaWeight(); //if (maxdw < 50) maxdw = 50; controller.ChangeWeights(mu / maxdw); ////sulymodositasok osszegzese //for (int i2 = 0; i2 < simCount; ++i2) //simCount //{ // int count = 0; // for (int i = 1; i < controllers[i2]; ++i) // { // foreach (INeuron n in controllers[i2].mlp[i]) // { // foreach (NeuronInput ni in ((Neuron)n).inputs) // { // if (deltaws.Count <= count) deltaws.Add(ni.deltaw); // else deltaws[count] += ni.deltaw; // ++count; // } // } // } //} ////legnagyobb sulymodositas ertekenek meghatarozasa, majd ezzel normalas //double maxdw = 1; //foreach (double dw in deltaws) //{ // if (Math.Abs(dw) > maxdw) maxdw = Math.Abs(dw); //} //if (maxdw < 50) maxdw = 50; ////sulymodositasok ervenyre juttatasa a controllerben //int count2 = 0; //for (int i = 1; i < controller.mlp.Count; ++i) //{ // foreach (INeuron n in controller.mlp[i]) // { // foreach (NeuronInput ni in ((Neuron)n).inputs) // { // ni.w += mu * deltaws[count2] / maxdw; // ++count2; // } // } //} SumSimCount = sumSimCount; return(error); }
public void SimulateOneStep(GridCarModelState state, out GridCarModelInput outInput, out GridCarModelState outState) { GridNeuralController.SimulateOneStep(this.controllerOriginal, this.model, state, out outInput, out outState); }
public static void SimulateOneStep(MLPDll controller, IGridModelSimulator model, GridCarModelState state, out GridCarModelInput outInput, out GridCarModelState outState) { double[] inputs = new double[5]; inputs[0] = ComMath.Normal(state.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, 0, MAX_NEURON_VALUE); inputs[1] = ComMath.Normal(state.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE); inputs[2] = ComMath.Normal(state.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE); inputs[3] = ComMath.Normal(state.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE); inputs[4] = ComMath.Normal(state.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE); double[] controllerOutputs = controller.Output(inputs); outInput = new GridCarModelInput(); outInput.Angle = ComMath.Normal(controllerOutputs[0], MIN_NEURON_VALUE, MAX_NEURON_VALUE, GridCarModelInput.MIN_ANGLE, GridCarModelInput.MAX_ANGLE); model.SimulateModel(state, outInput, out outState); }
public static void SimulateOneStep(MLPDll controller, IGridModelSimulator model, GridCarModelState state, out GridCarModelInput outInput, out GridCarModelState outState) { double[] inputs = new double[5]; inputs[0] = ComMath.Normal(state.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, 0, MAX_NEURON_VALUE); inputs[1] = ComMath.Normal(state.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE); inputs[2] = ComMath.Normal(state.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE); inputs[3] = ComMath.Normal(state.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE); inputs[4] = ComMath.Normal(state.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE); double[] controllerOutputs = controller.Output(inputs); outInput = new GridCarModelInput(); outInput.Angle = ComMath.Normal(controllerOutputs[0], MIN_NEURON_VALUE, MAX_NEURON_VALUE, GridCarModelInput.MIN_ANGLE, GridCarModelInput.MAX_ANGLE); model.SimulateModel(state, outInput, out outState); }
public void SimulateOneStep(GridCarModelState state, out GridCarModelInput outInput, out GridCarModelState outState) { GridNeuralController.SimulateOneStep(this.controllerOriginal, this.model, state, out outInput, out outState); }
public void Simulate(GridCarModelState initialState, int simCount, out GridCarModelInput[] inputs, out GridCarModelState[] states) { inputs = new GridCarModelInput[simCount]; states = new GridCarModelState[simCount]; GridCarModelState state = initialState; for (int i = 0; i < simCount; ++i) { SimulateOneStep(state, out inputs[i], out states[i]); state = states[i]; } }
//private double obstacleFieldError(GridCarModelState state) //{ // double err = 0; // List<ObstacleState> obstacles = obstacleProvider.GetObstacleStates(1); // foreach (ObstacleState obst in obstacles) // { // double d = ComMath.Normal(state.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, MIN_NEURON_VALUE, MAX_NEURON_VALUE); // double x = Math.Cos(state.TargetAngle - state.TargetFinishAngle) * d; // double y = Math.Sin(state.TargetAngle - state.TargetFinishAngle) * d; // double x0 = ComMath.Normal(obst.pp.position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE); // double y0 = ComMath.Normal(obst.pp.position.Y, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE); // double dist = Math.Sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0)); // err += Math.Pow(1 / dist - 1 / obst.radius, 2); // } // return err; //} private static PolarGrid obstaclePolarGrid(IObstaclePositionProvider ops, GridCarModelState state) { List<ObstacleState> obstacles = ops.GetObstacleStates(0); PolarGrid pg = new PolarGrid(); foreach (ObstacleState obst in obstacles) { GridObstacleState gos = GridObstacleState.FromObstacleState(obst, state); pg.AddObstacle(gos); } return pg; }
private void timer1_Tick(object sender, EventArgs e) { if (carRunning) { if (timerDiv == 0) { ICarPositionProvider carPos; IFinishPositionProvider finishPos; if (simulation) { itemManager.TakeSample(); carPos = itemManager; finishPos = itemManager; } else { cameraCarPosition.TakeSample(); carPos = cameraCarPosition; finishPos = cameraCarPosition; } //leallitas ha beert a celba double errx = carPos.GetCarState().Position.X - finishPos.GetFinishState(0).Position.X; double erry = carPos.GetCarState().Position.Y - finishPos.GetFinishState(0).Position.Y; double errox = carPos.GetCarState().Orientation.X - finishPos.GetFinishState(0).Orientation.X; double erroy = carPos.GetCarState().Orientation.Y - finishPos.GetFinishState(0).Orientation.Y; if ((errx * errx + erry * erry < CarModel.SHAFT_LENGTH * CarModel.SHAFT_LENGTH) && (errox * errox + erroy * erroy < 0.2)) { buttonStopSim_Click(this, null); } else { carModelGraphicControl1.SetReceiveCommand(); GridCarModelInput oi; GridCarModelState os; neuralController.SimulateOneStep(GridCarModelState.FromCarModelState(carPos.GetCarState()), out oi, out os); outState = GridCarModelState.ToCarModelState(os); outInput = new CarModelInput(oi.Angle); //outInput = new CarModelInput(20, 100); if (checkBoxSerial.Checked) { byte leftspd = (byte)Convert.ToSByte(ComMath.Normal(outInput.LeftSpeed, -180, 180, -128, 127)); byte rightspd = (byte)Convert.ToSByte(ComMath.Normal(outInput.RightSpeed, -180, 180, -128, 127)); //-125, 124 if (checkBoxCarEnable.Checked) { serialComm.Motor_I2C_Forward(1, leftspd, rightspd); } //Thread.Sleep(200); } } } timerDiv = (timerDiv + 1) % (long)(CarModel.SIMULATION_TIME_STEP * 1000.0 / timer1.Interval); if (simulation) { //itemManager.Simulate(new MathModelSimulator(), outInput, timer1.Interval / 1000.0); itemManager.SimualteGrid(new GridMathModelSimulator(), new GridCarModelInput(outInput.LeftSpeed, outInput.RightSpeed), timer1.Interval / 1000.0); } else { cameraCarPosition.Simulate(new MathModelSimulator(), outInput, timer1.Interval / 1000.0); } } carModelGraphicControl1.Invalidate(); }
private static double[] obstacleFieldErrorGradient(IObstaclePositionProvider ops, GridCarModelState state, int time) { //C = sum((1/d(X) - 1/d(0))^2) //dC/dy_x =... //dC/dy_y =... double ksi = 0.1; double disterr = 0; double orxerr = 0; double oryerr = 0; List <ObstacleState> obstacles = ops.GetObstacleStates(0); foreach (ObstacleState obst in obstacles)//cel az origo, tehat az origohoz relativak az akadalyok, origo felfele nez { double d = ComMath.Normal(Math.Sqrt(obst.pp.position.X * obst.pp.position.X + obst.pp.position.Y * obst.pp.position.Y), GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, 0, MAX_NEURON_VALUE); double a = ComMath.Normal(state.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, 0, MAX_NEURON_VALUE); double ang = Math.PI - (Math.Atan2(obst.pp.position.Y, obst.pp.position.X) + Math.PI) + state.TargetAngle - state.TargetFinishAngle; if (ang > Math.PI) { ang -= 2 * Math.PI; } if (ang < -Math.PI) { ang += 2 * Math.PI; } double AA = -2 * d * Math.Cos(ang); double BB = d * d; double obstdist = Math.Sqrt(a * a + BB + AA * a); double obstang = state.TargetAngle + Math.Sign(ang) * Math.Acos((a * a + obstdist * obstdist - d * d) / (2 * a * obstdist)); double r = ComMath.Normal(obst.radius + CarModel.SHAFT_LENGTH / 2, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, 0, MAX_NEURON_VALUE); double dist = obstdist - r; if (dist <= 0.0001) { dist = 0.0001; } double err = 1 / (2 * (dist * dist * dist)); disterr += -(AA + 2 * a) * err; double angerr = (-2 * a * d * Math.Sin(ang)) * err; orxerr += -Math.Sin(state.TargetAngle) * angerr; oryerr += Math.Cos(state.TargetAngle) * angerr; } if (obstacles.Count > 0) { disterr /= obstacles.Count; orxerr /= obstacles.Count; oryerr /= obstacles.Count; } return(new double[] { -ksi * disterr, -ksi * orxerr, -ksi * oryerr }); }
public void CalcErrorSensibility(double[] errors, out double[] sensitibility)//errors: distance error, orientation x,y error, finish orientation x,y error { GridCarModelState output1, output2, state1, state2, origiState = this.state; GridCarModelInput input1, input2, origiInput = this.input; double DIFF_C = 0.001; sensitibility = new double[6]; //****** //DIST //****** state1 = origiState; state1.TargetDist = ComMath.Normal(ComMath.Normal(state1.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST); this.SimulateModel(state1, origiInput, out output1); state2 = origiState; state2.TargetDist = ComMath.Normal(ComMath.Normal(state2.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST); this.SimulateModel(state2, origiInput, out output2); sensitibility[0] = (ComMath.Normal(output2.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] + (ComMath.Normal(output2.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] + (ComMath.Normal(output2.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] + (ComMath.Normal(output2.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3] + (ComMath.Normal(output2.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[4]; //****** //ORIENTATION X //****** state1 = origiState; state1.TargetOrientation = new PointD(ComMath.Normal(ComMath.Normal(state1.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY), state1.TargetOrientation.Y); this.SimulateModel(state1, origiInput, out output1); state2 = origiState; state2.TargetOrientation = new PointD(ComMath.Normal(ComMath.Normal(state2.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY), state2.TargetOrientation.Y); this.SimulateModel(state2, origiInput, out output2); sensitibility[1] = (ComMath.Normal(output2.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] + (ComMath.Normal(output2.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] + (ComMath.Normal(output2.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] + (ComMath.Normal(output2.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3] + (ComMath.Normal(output2.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[4]; //****** //ORIENTATION Y //****** state1 = origiState; state1.TargetOrientation = new PointD(state1.TargetOrientation.X, ComMath.Normal(ComMath.Normal(state1.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY)); this.SimulateModel(state1, origiInput, out output1); state2 = origiState; state2.TargetOrientation = new PointD(state2.TargetOrientation.X, ComMath.Normal(ComMath.Normal(state2.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY)); this.SimulateModel(state2, origiInput, out output2); sensitibility[2] = (ComMath.Normal(output2.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] + (ComMath.Normal(output2.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] + (ComMath.Normal(output2.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] + (ComMath.Normal(output2.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3] + (ComMath.Normal(output2.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[4]; //****** //FINISH ORIENTATION X //****** state1 = origiState; state1.TargetFinishOrientation = new PointD(ComMath.Normal(ComMath.Normal(state1.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY), state1.TargetFinishOrientation.Y); this.SimulateModel(state1, origiInput, out output1); state2 = origiState; state2.TargetFinishOrientation = new PointD(ComMath.Normal(ComMath.Normal(state2.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY), state2.TargetFinishOrientation.Y); this.SimulateModel(state2, origiInput, out output2); sensitibility[3] = (ComMath.Normal(output2.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] + (ComMath.Normal(output2.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] + (ComMath.Normal(output2.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] + (ComMath.Normal(output2.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3] + (ComMath.Normal(output2.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[4]; //****** //FINISH ORIENTATION Y //****** state1 = origiState; state1.TargetFinishOrientation = new PointD(state1.TargetFinishOrientation.X, ComMath.Normal(ComMath.Normal(state1.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY)); this.SimulateModel(state1, origiInput, out output1); state2 = origiState; state2.TargetFinishOrientation = new PointD(state2.TargetFinishOrientation.X, ComMath.Normal(ComMath.Normal(state2.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY)); this.SimulateModel(state2, origiInput, out output2); sensitibility[4] = (ComMath.Normal(output2.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] + (ComMath.Normal(output2.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] + (ComMath.Normal(output2.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] + (ComMath.Normal(output2.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3] + (ComMath.Normal(output2.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[4]; //****** //ANGLE //****** input1 = origiInput; input1.Angle = ComMath.Normal(ComMath.Normal(input1.Angle, GridCarModelInput.MIN_ANGLE, GridCarModelInput.MAX_ANGLE, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, GridCarModelInput.MIN_ANGLE, GridCarModelInput.MAX_ANGLE); this.SimulateModel(origiState, input1, out output1); input2 = origiInput; input2.Angle = ComMath.Normal(ComMath.Normal(input2.Angle, GridCarModelInput.MIN_ANGLE, GridCarModelInput.MAX_ANGLE, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, GridCarModelInput.MIN_ANGLE, GridCarModelInput.MAX_ANGLE); this.SimulateModel(origiState, input2, out output2); sensitibility[5] = (ComMath.Normal(output2.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] + (ComMath.Normal(output2.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] + (ComMath.Normal(output2.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] + (ComMath.Normal(output2.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3] + (ComMath.Normal(output2.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[4]; this.state = origiState; this.input = origiInput; }
//matematikai modell public void SimulateModel(GridCarModelState state, GridCarModelInput input, out GridCarModelState output) { SimulateModel(state, input, CarModel.SIMULATION_TIME_STEP, out output); }