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);
}