private void init(CarModelState state)
{
this.state = state;
int len = (int)(SHAFT_LENGTH / MM_PER_PIXEL / 2);
graphicCarModelWheelLeft = new PointF[] {
new PointF(-5, -len - 3),
new PointF(5, -len - 3),
new PointF(5, -len + 2),
new PointF(-5, -len + 2)
};
graphicCarModelWheelRight = new PointF[] {
new PointF(-5, len - 2),
new PointF(5, len - 2),
new PointF(5, len + 3),
new PointF(-5, len + 3)
};
graphicCarModelBody = new PointF[] {
new PointF(-1, -len + 2),
new PointF(-len * 2, -len + 2),
new PointF(-len * 2, len - 2),
new PointF(-1, len - 2)
};
}
public void Simulate(IModelSimulator modelsim, CarModelInput input, double timestep)
{
CarModel model = new CarModel(GetCarState());
model.SimulateModel(input, modelsim, timestep);
currentCarModelState = model.state;
}
public static void SimulateOneStep(MLPDll controller, IModelSimulator model, CarModelState state, out CarModelInput outInput, out CarModelState outState)
{
double[] inputs = new double[4];
inputs[0] = ComMath.Normal(state.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, POSITION_SCALE * MIN_NEURON_VALUE, POSITION_SCALE * MAX_NEURON_VALUE);
inputs[1] = ComMath.Normal(state.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, POSITION_SCALE * MIN_NEURON_VALUE, POSITION_SCALE * MAX_NEURON_VALUE);
inputs[2] = ComMath.Normal(state.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
inputs[3] = ComMath.Normal(state.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
double[] controllerOutputs = controller.Output(inputs);
if (INPUT_TYPE == inputType.wheelAngle)
{
outInput = new CarModelInput();
outInput.Angle = ComMath.Normal(controllerOutputs[0], MIN_NEURON_VALUE, MAX_NEURON_VALUE, CarModelInput.MIN_ANGLE, CarModelInput.MAX_ANGLE);
}
else if (INPUT_TYPE == inputType.wheelSpeed)
{
outInput = new CarModelInput(ComMath.Normal(controllerOutputs[0], MIN_NEURON_VALUE, MAX_NEURON_VALUE, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED),
ComMath.Normal(controllerOutputs[1], MIN_NEURON_VALUE, MAX_NEURON_VALUE, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED));
//********
//hatrafele tilos mennie
if (outInput.LeftSpeed < 0)
{
outInput.LeftSpeed = 0;
}
if (outInput.RightSpeed < 0)
{
outInput.RightSpeed = 0;
}
//********
}
model.SimulateModel(state, outInput, out outState);
}
public void SimulateModel(CarModelState state, CarModelInput input, double timeStep, out CarModelState output, out double[] NNOutput)
{
double[] inputs = new double[7];
if (NeuralController.INPUT_TYPE == inputType.wheelAngle)
{
inputs[6] = ComMath.Normal(input.Angle, CarModelInput.MIN_ANGLE, CarModelInput.MAX_ANGLE, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
}
else if (NeuralController.INPUT_TYPE == inputType.wheelSpeed)
{
inputs[4] = ComMath.Normal(input.LeftSpeed, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
inputs[5] = ComMath.Normal(input.RightSpeed, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
}
inputs[0] = ComMath.Normal(state.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
inputs[1] = ComMath.Normal(state.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
inputs[2] = ComMath.Normal(state.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
inputs[3] = ComMath.Normal(state.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
NNOutput = mlp.Output(inputs);
double X = ComMath.Normal(NNOutput[0], MIN_NEURON_VALUE, MAX_NEURON_VALUE, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X);
double Y = ComMath.Normal(NNOutput[1], MIN_NEURON_VALUE, MAX_NEURON_VALUE, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y);
double oX = ComMath.Normal(NNOutput[2], MIN_NEURON_VALUE, MAX_NEURON_VALUE, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY);
double oY = ComMath.Normal(NNOutput[3], MIN_NEURON_VALUE, MAX_NEURON_VALUE, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY);
output = new CarModelState(new PointD(X, Y), new PointD(oX, oY));
}
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 SimulationModeItemManager(ContextMenuStrip contextMenuStrip, CarModelGraphicControl graphicControl)
{
model = new CarModel(new CarModelState(new PointD(ComMath.Normal(0, 0, 1, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
ComMath.Normal(0, 0, 1, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)),
new PointD(0, 1)));
finish = new FinishModel(new PointD(ComMath.Normal(0.5, 0, 1, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
ComMath.Normal(0.5, 0, 1, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)),
0);
obstacles = new List<ObstacleModel>();
dragables = new List<IDragable>();
sampledCarState = model.state;
dragables.Add(model);
dragables.Add(finish);
dragged = null;
state = State.NONE;
this.contextMenuStrip = contextMenuStrip;
this.graphicControl = graphicControl;
contextMenuStrip.Items[0].Click += new EventHandler(newObstacle);
contextMenuStrip.Items[1].Click += new EventHandler(deleteObstacle);
}
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(CarModelState initialState, int simCount, out CarModelInput[] inputs, out CarModelState[] states)
{
inputs = new CarModelInput[simCount];
states = new CarModelState[simCount];
CarModelState state = initialState;
for (int i = 0; i < simCount; ++i)
{
SimulateOneStep(state, out inputs[i], out states[i]);
state = states[i];
}
}
public CameraObjectPositionProvider(PictureBox pb)
{
this.pb = pb;
camera = new CameraDirectShow();
camera.OnNewFrame += new OnNewFrameDelegate(camera_OnNewFrame);
camera.Start();
finishPredictor = new PositionAndOrientationPredictor(30, 10);
obstacles = new List <ObstacleModel>();
currentCarModelState = new CarModelState(new PointD(ComMath.Normal(0.05, 0, 1, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
ComMath.Normal(0.05, 0, 1, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)),
new PointD(0, 1));
currentFinishState = new FinishState(new PointD(ComMath.Normal(0.95, 0, 1, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
ComMath.Normal(0.95, 0, 1, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)),
0.5 * Math.PI);
}
public CameraObjectPositionProvider(PictureBox pb)
{
this.pb = pb;
camera = new CameraDirectShow();
camera.OnNewFrame += new OnNewFrameDelegate(camera_OnNewFrame);
camera.Start();
finishPredictor = new PositionAndOrientationPredictor(30, 10);
obstacles = new List<ObstacleModel>();
currentCarModelState = new CarModelState(new PointD(ComMath.Normal(0.05, 0, 1, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
ComMath.Normal(0.05, 0, 1, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)),
new PointD(0, 1));
currentFinishState = new FinishState(new PointD(ComMath.Normal(0.95, 0, 1, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
ComMath.Normal(0.95, 0, 1, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)),
0.5 * Math.PI);
}
//public NeuralModelSimulator(String filename)
//{
// mlp = new MLPDll(filename);
//}
public void Train(IModelSimulator sourceSimulator, double treshold)
{
Random r = new Random();
double mu = 0.0001;
long count = 0;
double errors = 0, errors2 = double.MaxValue;
double[] error = new double[4];
do
{
for (int i2 = 0; i2 < EPOCH_COUNT; ++i2)
{
double angle = r.NextDouble() * 2 * Math.PI;//veletlen szog
CarModelState carstate = new CarModelState(new PointD(ComMath.Normal(r.NextDouble(), 0, 1, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
ComMath.Normal(r.NextDouble(), 0, 1, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)),
new PointD(ComMath.Normal(Math.Cos(angle), -1, 1, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY),
ComMath.Normal(Math.Sin(angle), -1, 1, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY)));
CarModelInput carinput = new CarModelInput();
//= new CarModelInput(ComMath.Normal(r.NextDouble(), 0, 1, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED),
// ComMath.Normal(r.NextDouble(), 0, 1, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED));
carinput.Angle = ComMath.Normal(r.NextDouble(), 0, 1, CarModelInput.MIN_ANGLE, CarModelInput.MAX_ANGLE);
CarModelState state, state2;
double[] output;
sourceSimulator.SimulateModel(carstate, carinput, out state);
this.SimulateModel(carstate, carinput, out state2, out output);
error = new double[4];
error[0] = -output[0] + ComMath.Normal(state.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
error[1] = -output[1] + ComMath.Normal(state.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
error[2] = -output[2] + ComMath.Normal(state.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
error[3] = -output[3] + ComMath.Normal(state.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
count++;
mlp.Train(mu, error);
errors += error[0] * error[0] + error[1] * error[1] + error[2] * error[2] + error[3] * error[3];
}
errors /= EPOCH_COUNT;
//if (errors2 < errors) mu *= 0.75;
errors2 = errors;
System.Console.WriteLine(errors.ToString());
} while (errors > treshold);
// mlp.SaveNN("neuralmodel.mlp");
}
public void SimulateModel(CarModelState state, CarModelInput input, double timeStep, out CarModelState output)
{
this.state = state;
this.input = input;
CarModelState 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.Angle += dAngle;
PointD p = new PointD((state.Position.X + vectLength * Math.Cos(state2.Angle - dAngle / 2)),
(state.Position.Y + vectLength * Math.Sin(state2.Angle - dAngle / 2)));
state2.Position = p;
output = state2;
}
//public NeuralModelSimulator(String filename)
//{
// mlp = new MLPDll(filename);
//}
public void Train(IModelSimulator sourceSimulator, double treshold)
{
Random r = new Random();
double mu = 0.0001;
long count = 0;
double errors = 0, errors2 = double.MaxValue;
double[] error = new double[4];
do
{
for (int i2 = 0; i2 < EPOCH_COUNT; ++i2)
{
double angle = r.NextDouble() * 2 * Math.PI;//veletlen szog
CarModelState carstate = new CarModelState(new PointD(ComMath.Normal(r.NextDouble(), 0, 1, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
ComMath.Normal(r.NextDouble(), 0, 1, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)),
new PointD(ComMath.Normal(Math.Cos(angle), -1, 1, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY),
ComMath.Normal(Math.Sin(angle), -1, 1, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY)));
CarModelInput carinput = new CarModelInput();
//= new CarModelInput(ComMath.Normal(r.NextDouble(), 0, 1, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED),
// ComMath.Normal(r.NextDouble(), 0, 1, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED));
carinput.Angle = ComMath.Normal(r.NextDouble(), 0, 1, CarModelInput.MIN_ANGLE, CarModelInput.MAX_ANGLE);
CarModelState state, state2;
double[] output;
sourceSimulator.SimulateModel(carstate, carinput, out state);
this.SimulateModel(carstate, carinput, out state2, out output);
error = new double[4];
error[0] = -output[0] + ComMath.Normal(state.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
error[1] = -output[1] + ComMath.Normal(state.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
error[2] = -output[2] + ComMath.Normal(state.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
error[3] = -output[3] + ComMath.Normal(state.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
count++;
mlp.Train(mu, error);
errors += error[0] * error[0] + error[1] * error[1] + error[2] * error[2] + error[3] * error[3];
}
errors /= EPOCH_COUNT;
//if (errors2 < errors) mu *= 0.75;
errors2 = errors;
System.Console.WriteLine(errors.ToString());
} while (errors > treshold);
// mlp.SaveNN("neuralmodel.mlp");
}
private double obstacleFieldError(CarModelState state)
{
double err = 0;
List <ObstacleState> obstacles = obstacleProvider.GetObstacleStates(1);
foreach (ObstacleState obst in obstacles)
{
double x = ComMath.Normal(state.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
double y = ComMath.Normal(state.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
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_Y, CarModelState.MAX_POS_Y, 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);
}
//public NeuralController(IModelSimulator model, IObstaclePositionProvider obstacle, ICarPositionProvider start, IFinishPositionProvider finish, string filename)
//{
// this.model = model;
// controller = new MLPDll(filename);
// obstacleProvider = obstacle;
// carStateProvider = start;
// finishStateProvider = finish;
// trainingStopped = true;
// mu = 0.005;
//}
public Bitmap VisualizeObstacleField(int width, int height)
{
lock (this)
{
uint[] buf = new uint[width * height];
for (int x = 0; x < width; ++x)
{
for (int y = 0; y < height; ++y)
{
CarModelState state = new CarModelState();
state.Angle = 0;
state.Position = new PointD(ComMath.Normal(x, 0, width - 1, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
ComMath.Normal(y, 0, height - 1, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y));
double err = obstacleFieldError(state);
if (err > 255)
{
err = 255;
}
buf[y * width + x] = 0x80000000 + ((uint)(err));
}
}
Bitmap bm = new Bitmap(width, height, PixelFormat.Format32bppArgb);
BitmapData bmData = bm.LockBits(new Rectangle(0, 0, width, height), ImageLockMode.ReadWrite, PixelFormat.Format32bppArgb);
int len = bmData.Width * bmData.Height;
unsafe
{
uint *cim = (uint *)bmData.Scan0.ToPointer();//direkt bitmap memoriaba rajzolunk, gyors
for (int i = 0; i < len; ++i)
{
cim[i] = buf[i];
}
}
bm.UnlockBits(bmData);
return(bm);
}
}
public void SimulateModel(CarModelState state, CarModelInput input, double timeStep, out CarModelState output)
{
this.state = state;
this.input = input;
CarModelState 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.Angle += dAngle;
PointD p = new PointD((state.Position.X + vectLength * Math.Cos(state2.Angle - dAngle / 2)),
(state.Position.Y + vectLength * Math.Sin(state2.Angle - dAngle / 2)));
state2.Position = p;
output = state2;
}
private double[] obstacleFieldErrorGradient(CarModelState state, int time)
{
//C = sum((1/d(X) - 1/d(0))^2)
//dC/dy_x =...
//dC/dy_y =...
double ksi = 0.0001;
double errX = 0;
double errY = 0;
List <ObstacleState> obstacles = obstacleProvider.GetObstacleStates(time);
foreach (ObstacleState obst in obstacles)
{
double x = ComMath.Normal(state.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
double y = ComMath.Normal(state.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
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_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
double r = ComMath.Normal(obst.radius + CarModel.SHAFT_LENGTH / 2, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
double dist = Math.Sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0)) - r;
if (dist <= 0.0001)
{
dist = 0.0001;
}
double err = (1 / (dist * dist * dist));
errX += err * (x - x0);
errY += err * (y - y0);
}
if (obstacles.Count > 0)
{
errX /= obstacles.Count;
errY /= obstacles.Count;
}
return(new double[] { ksi *errX, ksi *errY, 0, 0 });
}
private void init(CarModelState state)
{
this.state = state;
int len = (int)(SHAFT_LENGTH / MM_PER_PIXEL / 2);
graphicCarModelWheelLeft = new PointF[] {
new PointF(-5, -len - 3),
new PointF(5, -len - 3),
new PointF(5, -len + 2),
new PointF(-5, -len + 2)
};
graphicCarModelWheelRight = new PointF[] {
new PointF(-5, len - 2),
new PointF(5, len - 2),
new PointF(5, len + 3),
new PointF(-5, len + 3)
};
graphicCarModelBody = new PointF[] {
new PointF(-1, -len + 2),
new PointF(-len*2, -len + 2),
new PointF(-len*2, len - 2),
new PointF(-1, len - 2)
};
}
public void SimulateModel(CarModelState state, CarModelInput input, out CarModelState output, out double[] NNOutput)
{
SimulateModel(state, input, CarModel.SIMULATION_TIME_STEP, out output, out NNOutput);
}
//nem jo
/*
public void CalcErrorSensibility(double[] errors, out double[] sensitibility)
{
double dAngle = (input.RightSpeed - input.LeftSpeed) * CarModel.SIMULATION_TIME_STEP / CarModel.SHAFT_LENGTH;
double lamda = 1;
if (dAngle != 0) lamda = 4 / dAngle * Math.Sin(dAngle / 2);
double vectLength = (input.RightSpeed + input.LeftSpeed) / 2 * CarModel.SIMULATION_TIME_STEP * lamda;
//ez felfoghato egy forgataskent is:
//vectLength * (state.Orientation.X *Math.Cos(-dAngle / 2) - state.Orientation.Y * Math.Sin(-dAngle / 2))
//vectLength * (state.Orientation.X *Math.Sin(-dAngle / 2) + state.Orientation.Y * Math.Cos(-dAngle / 2))
//
PointD p = new PointD((state.Position.X + vectLength * Math.Cos(state.Angle - dAngle / 2)),
(state.Position.Y + vectLength * Math.Sin(state.Angle - dAngle / 2)));
//kimenetek bemenet szerinti derivaltjai
double dAngle_rightSpeed = CarModel.SIMULATION_TIME_STEP / CarModel.SHAFT_LENGTH;
double dAngle_leftSpeed = -CarModel.SIMULATION_TIME_STEP / CarModel.SHAFT_LENGTH;
double lamda_rightSpeed = 0;
double lamda_leftSpeed = 0;
if (dAngle != 0)
{
lamda_rightSpeed = dAngle_rightSpeed * (-4 / Math.Pow(dAngle, 2) * Math.Sin(dAngle / 2) + 2 / dAngle * Math.Cos(dAngle / 2));
lamda_leftSpeed = dAngle_leftSpeed * (-4 / Math.Pow(dAngle, 2) * Math.Sin(dAngle / 2) + 2 / dAngle * Math.Cos(dAngle / 2));
}
double vectLength_rightSpeed = lamda_rightSpeed * 1 / 2 * CarModel.SIMULATION_TIME_STEP * lamda;
double vectLength_leftSpeed = lamda_leftSpeed * 1 / 2 * CarModel.SIMULATION_TIME_STEP * lamda;
double outposX_inposX = 1;
double outposX_inposY = 0;
double outposX_inangX = vectLength * Math.Cos(- dAngle / 2);
double outposX_inangY = - vectLength * Math.Sin(- dAngle / 2);
double outposX_inrightSpeed = vectLength_rightSpeed * (state.Orientation.X *Math.Cos(-dAngle / 2) - state.Orientation.Y * Math.Sin(-dAngle / 2)) + dAngle_rightSpeed * vectLength * (-1 / 2) * (state.Orientation.X *(-Math.Sin(-dAngle / 2)) - state.Orientation.Y *Math.Cos(-dAngle / 2));
double outposX_inleftSpeed = vectLength_leftSpeed * (state.Orientation.X *Math.Cos(-dAngle / 2) - state.Orientation.Y * Math.Sin(-dAngle / 2)) + dAngle_leftSpeed * vectLength * (-1 / 2) * (state.Orientation.X *(-Math.Sin(-dAngle / 2)) + state.Orientation.Y *Math.Cos(-dAngle / 2));
double outposY_inposX = 0;
double outposY_inposY = 1;
double outposY_inangX = vectLength * Math.Sin(- dAngle / 2);
double outposY_inangY = vectLength * Math.Cos(- dAngle / 2);
double outposY_inrightSpeed = vectLength_rightSpeed * (state.Orientation.X *Math.Sin(-dAngle / 2) + state.Orientation.Y * Math.Cos(-dAngle / 2)) + dAngle_rightSpeed * vectLength * (-1 / 2) * (state.Orientation.X *Math.Cos(-dAngle / 2) - state.Orientation.Y *Math.Sin(-dAngle / 2));
double outposY_inleftSpeed = vectLength_leftSpeed * (state.Orientation.X * Math.Sin(-dAngle / 2) + state.Orientation.Y * Math.Cos(-dAngle / 2)) + dAngle_leftSpeed * vectLength * (-1 / 2) * (state.Orientation.X * Math.Cos(-dAngle / 2) - state.Orientation.Y * Math.Sin(-dAngle / 2));
double outangX_inposX = 0;
double outangX_inposY = 0;
double outangX_inangX = Math.Cos(dAngle);
double outangX_inangY = -Math.Sin(dAngle);
double outangX_inrightSpeed = dAngle_rightSpeed * (state.Orientation.X * (-Math.Sin(dAngle)) + state.Orientation.Y * (-Math.Cos(dAngle)));
double outangX_inleftSpeed = dAngle_leftSpeed * (state.Orientation.X * (-Math.Sin(dAngle)) + state.Orientation.Y * (-Math.Cos(dAngle)));
double outangY_inposX = 0;
double outangY_inposY = 0;
double outangY_inangX = Math.Sin(dAngle);
double outangY_inangY = Math.Cos(dAngle);
double outangY_inrightSpeed = dAngle_rightSpeed * (state.Orientation.X * Math.Cos(dAngle) - state.Orientation.Y * Math.Sin(dAngle));
double outangY_inleftSpeed = dAngle_leftSpeed * (state.Orientation.X * Math.Cos(dAngle) - state.Orientation.Y * Math.Sin(dAngle));
sensitibility = new double[6];
sensitibility[0] = (outposX_inposX * errors[0] + outposY_inposX * errors[1] + outangX_inposX * errors[2] + outangY_inposX * errors[3]);
sensitibility[1] = (outposX_inposY * errors[0] + outposY_inposY * errors[1] + outangX_inposY * errors[2] + outangY_inposY * errors[3]);
sensitibility[2] = (outposX_inangX * errors[0] + outposY_inangX * errors[1] + outangX_inangX * errors[2] + outangY_inangX * errors[3]);
sensitibility[3] = (outposX_inangY * errors[0] + outposY_inangY * errors[1] + outangX_inangY * errors[2] + outangY_inangY * errors[3]);
sensitibility[4] = (outposX_inrightSpeed * errors[0] + outposY_inrightSpeed * errors[1] + outangX_inrightSpeed * errors[2] + outangY_inrightSpeed * errors[3]);
sensitibility[5] = (outposX_inleftSpeed * errors[0] + outposY_inleftSpeed * errors[1] + outangX_inleftSpeed * errors[2] + outangY_inleftSpeed * errors[3]);
}
*/
public void CalcErrorSensibility(double[] errors, out double[] sensitibility)
{
CarModelState output1, output2, state1, state2, origiState = this.state;
CarModelInput input1, input2, origiInput = this.input;
double DIFF_C = 0.001;
sensitibility = new double[7];
//******
//POS X
//******
state1 = origiState;
state1.Position = new PointD(ComMath.Normal(ComMath.Normal(state1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
state1.Position.Y);
this.SimulateModel(state1, origiInput, out output1);
state2 = origiState;
state2.Position = new PointD(ComMath.Normal(ComMath.Normal(state2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
state2.Position.Y);
this.SimulateModel(state2, origiInput, out output2);
sensitibility[0] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] +
(ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] +
(ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] +
(ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3];
//******
//POS Y
//******
state1 = origiState;
state1.Position = new PointD(state1.Position.X,
ComMath.Normal(ComMath.Normal(state1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y));
this.SimulateModel(state1, origiInput, out output1);
state2 = origiState;
state2.Position = new PointD(state2.Position.X,
ComMath.Normal(ComMath.Normal(state2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y));
this.SimulateModel(state2, origiInput, out output2);
sensitibility[1] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] +
(ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] +
(ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] +
(ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3];
//******
//ORIENTATION X
//******
state1 = origiState;
state1.Orientation = new PointD(ComMath.Normal(ComMath.Normal(state1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY),
state1.Orientation.Y);
this.SimulateModel(state1, origiInput, out output1);
state2 = origiState;
state2.Orientation = new PointD(ComMath.Normal(ComMath.Normal(state2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY),
state2.Orientation.Y);
this.SimulateModel(state2, origiInput, out output2);
sensitibility[2] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] +
(ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] +
(ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] +
(ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3];
//******
//ORIENTATION Y
//******
state1 = origiState;
state1.Orientation = new PointD(state1.Orientation.X,
ComMath.Normal(ComMath.Normal(state1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY));
this.SimulateModel(state1, origiInput, out output1);
state2 = origiState;
state2.Orientation = new PointD(state2.Orientation.X,
ComMath.Normal(ComMath.Normal(state2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY));
this.SimulateModel(state2, origiInput, out output2);
sensitibility[3] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] +
(ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] +
(ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] +
(ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3];
//******
//LEFT SPEED
//******
input1 = origiInput;
input1.LeftSpeed = ComMath.Normal(ComMath.Normal(input1.LeftSpeed, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED);
this.SimulateModel(origiState, input1, out output1);
input2 = origiInput;
input2.LeftSpeed = ComMath.Normal(ComMath.Normal(input2.LeftSpeed, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED);
this.SimulateModel(origiState, input2, out output2);
sensitibility[4] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] +
(ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] +
(ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] +
(ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3];
//******
//RIGHT SPEED
//******
input1 = origiInput;
input1.RightSpeed = ComMath.Normal(ComMath.Normal(input1.RightSpeed, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED);
this.SimulateModel(origiState, input1, out output1);
input2 = origiInput;
input2.RightSpeed = ComMath.Normal(ComMath.Normal(input2.RightSpeed, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED);
this.SimulateModel(origiState, input2, out output2);
sensitibility[5] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] +
(ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] +
(ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] +
(ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3];
//******
//WHEEL ANGLE
//******
input1 = origiInput;
input1.Angle = ComMath.Normal(ComMath.Normal(input1.Angle, CarModelInput.MIN_ANGLE, CarModelInput.MAX_ANGLE,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelInput.MIN_ANGLE, CarModelInput.MAX_ANGLE);
this.SimulateModel(origiState, input1, out output1);
input2 = origiInput;
input2.Angle = ComMath.Normal(ComMath.Normal(input2.Angle, CarModelInput.MIN_ANGLE, CarModelInput.MAX_ANGLE,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelInput.MIN_ANGLE, CarModelInput.MAX_ANGLE);
this.SimulateModel(origiState, input2, out output2);
sensitibility[6] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] +
(ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] +
(ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] +
(ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3];
this.state = origiState;
this.input = origiInput;
}
public void SimulateModel(CarModelState state, CarModelInput input, double timeStep, out CarModelState output)
{
double[] NNout;
SimulateModel(state, input, timeStep, out output, out NNout);
}
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 void RunMarkerFinder()
{
if (mf != null)
{
Bitmap frame = camera.GetBitmap();
List <Shape> shapes = null;
shapes = mf.ProcessFrame(frame);
bool carFound = false;
bool finishFound = false;
// finish
currentObstacleStates = new List <ObstacleState>();
foreach (Shape s in shapes)
{
if ((s.index == 0) && (s.scale > 0.17) && (s.scale < 0.22))
{
currentCarModelState = new CarModelState(new PointD(ComMath.Normal(s.pos.X, 0, frame.Width, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
ComMath.Normal(s.pos.Y, 0, frame.Height, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)),
-Math.PI / 2 - s.rot);
carFound = true;
}
else if ((s.index == 1) && (s.scale > 0.16) && (s.scale < 0.20))
{
currentFinishState = new FinishState(new PointD(ComMath.Normal(s.pos.X, 0, frame.Width, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
ComMath.Normal(s.pos.Y, 0, frame.Height, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)),
-Math.PI / 2 - s.rot);
finishPredictor.AddPoint(currentFinishState.Position, currentFinishState.Orientation);
finishFound = true;
}
else
{
currentObstacleStates.Add(new ObstacleState(new PointD(ComMath.Normal(s.pos.X, 0, frame.Width, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
ComMath.Normal(s.pos.Y, 0, frame.Height, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)),
s.scale * 500));
//ide kell egy robusztus alg
//ha zaj jon, akkor ne vegye be
}
}
lock (obstacles)
{
List <ObstacleState> curObState = new List <ObstacleState>(currentObstacleStates);
foreach (ObstacleModel ops in obstacles)
{
List <PointD> list = ops.state.pp.PredictNextPositions(1);
ObstacleState osw = null;
PointD pd;
if (list != null)
{
pd = list[0];
}
else
{
pd = ops.state.pp.position;
}
double mindist = double.MaxValue;
foreach (ObstacleState os in curObState)
{
double dist = (os.pp.position.X - pd.X) * (os.pp.position.X - pd.X) + (os.pp.position.Y - pd.Y) * (os.pp.position.Y - pd.Y);
if (dist < mindist)
{
mindist = dist;
osw = os;
}
}
if (osw != null)
{
lock (ops)
{
ops.state.pp.AddNewPosition(osw.pp.position);
}
curObState.Remove(osw);
}
else
{
//a predikcioval leptetjuk tovabb
lock (ops)
{
ops.state.pp.AddNewPosition(pd);
}
}
}
foreach (ObstacleState os in curObState)
{
lock (obstacles)
{
ObstacleModel om = new ObstacleModel(os.pp.position, os.radius);
om.SetSelectedState(1, 0);
if (obstacles.Count < 1)
{
obstacles.Add(om);
}
}
}
}
}
}
//public CarModelState GetState(int index)
//{
// double[] ret = GetPosition(index);
// CarModelState cs = new CarModelState();
// cs.Angle = -ret[2];
// //cs.Position = new PointD(ComMath.Normal(ret[0], 0, im.Width, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X), ComMath.Normal(im.Height - ret[1], 0, im.Height, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y));
// return cs;
//}
public void TakeSample()
{
RunMarkerFinder();
sampledCarState = currentCarModelState;
}
private double[] obstacleFieldErrorGradient(CarModelState state, int time)
{
//C = sum((1/d(X) - 1/d(0))^2)
//dC/dy_x =...
//dC/dy_y =...
double ksi = 0.0001;
double errX = 0;
double errY = 0;
List<ObstacleState> obstacles = obstacleProvider.GetObstacleStates(time);
foreach (ObstacleState obst in obstacles)
{
double x = ComMath.Normal(state.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
double y = ComMath.Normal(state.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
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_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
double r = ComMath.Normal(obst.radius + CarModel.SHAFT_LENGTH/2, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
double dist = Math.Sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0)) - r;
if (dist <= 0.0001) dist = 0.0001;
double err = (1 / (dist * dist * dist));
errX += err * (x - x0);
errY += err * (y - y0);
}
if (obstacles.Count > 0)
{
errX /= obstacles.Count;
errY /= obstacles.Count;
}
return new double[] { ksi * errX, ksi * errY, 0, 0 };
}
//matematikai modell
public void SimulateModel(CarModelState state, CarModelInput input, out CarModelState output)
{
SimulateModel(state, input, CarModel.SIMULATION_TIME_STEP, out output);
}
public void SimulateModel(CarModelState state, CarModelInput input, double timeStep, out CarModelState output)
{
double[] NNout;
SimulateModel(state, input, timeStep, out output, out NNout);
}
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();
}
public void SimulateOneStep(CarModelState state, out CarModelInput outInput, out CarModelState outState)
{
NeuralController.SimulateOneStep(this.controller, this.model, state, out outInput, out outState);
}
public CarModel(CarModelState state, Color c)
{
mainColor = c;
init(state);
}
private double TrainOneEpoch(double mu, out double SumSimCount, out List <CarModelState> innerStates)
{
int maxSimCount = 100;
double sumSimCount = 0;
double error = 0;
innerStates = new List <CarModelState>();
List <double> deltaws = new List <double>();
MLPDll[] controllers = new MLPDll[maxSimCount];
IModelSimulator[] models = new IModelSimulator[maxSimCount];
CarModelState state = carStateProvider.GetCarState();
CarModelInput input = new CarModelInput();
//kimenet kiszamitasa
int simCount = 0;
List <double[]> singleErrors = new List <double[]>();
List <double[]> regularizationErrors = new List <double[]>();
CarModelState laststate;
bool earlyStop;
do
{
controllers[simCount] = new MLPDll(controller); //lemasoljuk
models[simCount] = model.Clone(); //a modellt is
laststate = state;
NeuralController.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(state, simCount));
//minden pont celtol vett tavolsaga
double[] desiredOutput = (double[])finishStateProvider.GetFinishState(simCount);
singleErrors.Add(new double[] { 1 * ComMath.Normal(desiredOutput[0] - state.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE),
1 * ComMath.Normal(desiredOutput[1] - state.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE),
0.1 * ComMath.Normal(desiredOutput[2] - state.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE),
0.1 * ComMath.Normal(desiredOutput[3] - state.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.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 < err1.Length; i++)
{
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;
if (INPUT_TYPE == inputType.wheelAngle)
{
inputSensitibility = new double[1];
inputSensitibility[0] = sensitibility[6];
}
else if (INPUT_TYPE == inputType.wheelSpeed)
{
inputSensitibility = new double[2];
inputSensitibility[0] = sensitibility[4];
inputSensitibility[1] = sensitibility[5];
}
double[] sensitibility2;
controllers[i].SetOutputError(inputSensitibility);
controllers[i].Backpropagate();
controllers[i].CalculateDeltaWeights();
sensitibility2 = controllers[i].SensitibilityD();
errors[0] = (sensitibility[0] + sensitibility2[0]);
errors[1] = (sensitibility[1] + sensitibility2[1]);
errors[2] = (sensitibility[2] + sensitibility2[2]);
errors[3] = (sensitibility[3] + sensitibility2[3]);
//regularizaciobol szarmazo hiba hozzaadasa
errors[0] += regularizationErrors[i][0];
errors[1] += regularizationErrors[i][1];
}
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 SimulateModel(CarModelState state, CarModelInput input, double timeStep, out CarModelState output, out double[] NNOutput)
{
double[] inputs = new double[7];
if (NeuralController.INPUT_TYPE == inputType.wheelAngle)
{
inputs[6] = ComMath.Normal(input.Angle, CarModelInput.MIN_ANGLE, CarModelInput.MAX_ANGLE, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
}
else if (NeuralController.INPUT_TYPE == inputType.wheelSpeed)
{
inputs[4] = ComMath.Normal(input.LeftSpeed, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
inputs[5] = ComMath.Normal(input.RightSpeed, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
}
inputs[0] = ComMath.Normal(state.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
inputs[1] = ComMath.Normal(state.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
inputs[2] = ComMath.Normal(state.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
inputs[3] = ComMath.Normal(state.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
NNOutput = mlp.Output(inputs);
double X = ComMath.Normal(NNOutput[0], MIN_NEURON_VALUE, MAX_NEURON_VALUE, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X);
double Y = ComMath.Normal(NNOutput[1], MIN_NEURON_VALUE, MAX_NEURON_VALUE, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y);
double oX = ComMath.Normal(NNOutput[2], MIN_NEURON_VALUE, MAX_NEURON_VALUE, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY);
double oY = ComMath.Normal(NNOutput[3], MIN_NEURON_VALUE, MAX_NEURON_VALUE, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY);
output = new CarModelState(new PointD(X, Y), new PointD(oX, oY));
}
public CarModel(CarModelState state)
{
mainColor = Color.Gray;
init(state);
}
//nem jo
/*
* public void CalcErrorSensibility(double[] errors, out double[] sensitibility)
* {
* double dAngle = (input.RightSpeed - input.LeftSpeed) * CarModel.SIMULATION_TIME_STEP / CarModel.SHAFT_LENGTH;
* double lamda = 1;
* if (dAngle != 0) lamda = 4 / dAngle * Math.Sin(dAngle / 2);
* double vectLength = (input.RightSpeed + input.LeftSpeed) / 2 * CarModel.SIMULATION_TIME_STEP * lamda;
*
*
* //ez felfoghato egy forgataskent is:
* //vectLength * (state.Orientation.X *Math.Cos(-dAngle / 2) - state.Orientation.Y * Math.Sin(-dAngle / 2))
* //vectLength * (state.Orientation.X *Math.Sin(-dAngle / 2) + state.Orientation.Y * Math.Cos(-dAngle / 2))
* //
* PointD p = new PointD((state.Position.X + vectLength * Math.Cos(state.Angle - dAngle / 2)),
* (state.Position.Y + vectLength * Math.Sin(state.Angle - dAngle / 2)));
*
* //kimenetek bemenet szerinti derivaltjai
* double dAngle_rightSpeed = CarModel.SIMULATION_TIME_STEP / CarModel.SHAFT_LENGTH;
* double dAngle_leftSpeed = -CarModel.SIMULATION_TIME_STEP / CarModel.SHAFT_LENGTH;
* double lamda_rightSpeed = 0;
* double lamda_leftSpeed = 0;
* if (dAngle != 0)
* {
* lamda_rightSpeed = dAngle_rightSpeed * (-4 / Math.Pow(dAngle, 2) * Math.Sin(dAngle / 2) + 2 / dAngle * Math.Cos(dAngle / 2));
* lamda_leftSpeed = dAngle_leftSpeed * (-4 / Math.Pow(dAngle, 2) * Math.Sin(dAngle / 2) + 2 / dAngle * Math.Cos(dAngle / 2));
* }
* double vectLength_rightSpeed = lamda_rightSpeed * 1 / 2 * CarModel.SIMULATION_TIME_STEP * lamda;
* double vectLength_leftSpeed = lamda_leftSpeed * 1 / 2 * CarModel.SIMULATION_TIME_STEP * lamda;
*
*
* double outposX_inposX = 1;
* double outposX_inposY = 0;
* double outposX_inangX = vectLength * Math.Cos(- dAngle / 2);
* double outposX_inangY = - vectLength * Math.Sin(- dAngle / 2);
* double outposX_inrightSpeed = vectLength_rightSpeed * (state.Orientation.X *Math.Cos(-dAngle / 2) - state.Orientation.Y * Math.Sin(-dAngle / 2)) + dAngle_rightSpeed * vectLength * (-1 / 2) * (state.Orientation.X *(-Math.Sin(-dAngle / 2)) - state.Orientation.Y *Math.Cos(-dAngle / 2));
* double outposX_inleftSpeed = vectLength_leftSpeed * (state.Orientation.X *Math.Cos(-dAngle / 2) - state.Orientation.Y * Math.Sin(-dAngle / 2)) + dAngle_leftSpeed * vectLength * (-1 / 2) * (state.Orientation.X *(-Math.Sin(-dAngle / 2)) + state.Orientation.Y *Math.Cos(-dAngle / 2));
*
* double outposY_inposX = 0;
* double outposY_inposY = 1;
* double outposY_inangX = vectLength * Math.Sin(- dAngle / 2);
* double outposY_inangY = vectLength * Math.Cos(- dAngle / 2);
* double outposY_inrightSpeed = vectLength_rightSpeed * (state.Orientation.X *Math.Sin(-dAngle / 2) + state.Orientation.Y * Math.Cos(-dAngle / 2)) + dAngle_rightSpeed * vectLength * (-1 / 2) * (state.Orientation.X *Math.Cos(-dAngle / 2) - state.Orientation.Y *Math.Sin(-dAngle / 2));
* double outposY_inleftSpeed = vectLength_leftSpeed * (state.Orientation.X * Math.Sin(-dAngle / 2) + state.Orientation.Y * Math.Cos(-dAngle / 2)) + dAngle_leftSpeed * vectLength * (-1 / 2) * (state.Orientation.X * Math.Cos(-dAngle / 2) - state.Orientation.Y * Math.Sin(-dAngle / 2));
*
*
* double outangX_inposX = 0;
* double outangX_inposY = 0;
* double outangX_inangX = Math.Cos(dAngle);
* double outangX_inangY = -Math.Sin(dAngle);
* double outangX_inrightSpeed = dAngle_rightSpeed * (state.Orientation.X * (-Math.Sin(dAngle)) + state.Orientation.Y * (-Math.Cos(dAngle)));
* double outangX_inleftSpeed = dAngle_leftSpeed * (state.Orientation.X * (-Math.Sin(dAngle)) + state.Orientation.Y * (-Math.Cos(dAngle)));
*
* double outangY_inposX = 0;
* double outangY_inposY = 0;
* double outangY_inangX = Math.Sin(dAngle);
* double outangY_inangY = Math.Cos(dAngle);
* double outangY_inrightSpeed = dAngle_rightSpeed * (state.Orientation.X * Math.Cos(dAngle) - state.Orientation.Y * Math.Sin(dAngle));
* double outangY_inleftSpeed = dAngle_leftSpeed * (state.Orientation.X * Math.Cos(dAngle) - state.Orientation.Y * Math.Sin(dAngle));
*
* sensitibility = new double[6];
* sensitibility[0] = (outposX_inposX * errors[0] + outposY_inposX * errors[1] + outangX_inposX * errors[2] + outangY_inposX * errors[3]);
* sensitibility[1] = (outposX_inposY * errors[0] + outposY_inposY * errors[1] + outangX_inposY * errors[2] + outangY_inposY * errors[3]);
* sensitibility[2] = (outposX_inangX * errors[0] + outposY_inangX * errors[1] + outangX_inangX * errors[2] + outangY_inangX * errors[3]);
* sensitibility[3] = (outposX_inangY * errors[0] + outposY_inangY * errors[1] + outangX_inangY * errors[2] + outangY_inangY * errors[3]);
* sensitibility[4] = (outposX_inrightSpeed * errors[0] + outposY_inrightSpeed * errors[1] + outangX_inrightSpeed * errors[2] + outangY_inrightSpeed * errors[3]);
* sensitibility[5] = (outposX_inleftSpeed * errors[0] + outposY_inleftSpeed * errors[1] + outangX_inleftSpeed * errors[2] + outangY_inleftSpeed * errors[3]);
* }
*/
public void CalcErrorSensibility(double[] errors, out double[] sensitibility)
{
CarModelState output1, output2, state1, state2, origiState = this.state;
CarModelInput input1, input2, origiInput = this.input;
double DIFF_C = 0.001;
sensitibility = new double[7];
//******
//POS X
//******
state1 = origiState;
state1.Position = new PointD(ComMath.Normal(ComMath.Normal(state1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
state1.Position.Y);
this.SimulateModel(state1, origiInput, out output1);
state2 = origiState;
state2.Position = new PointD(ComMath.Normal(ComMath.Normal(state2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
state2.Position.Y);
this.SimulateModel(state2, origiInput, out output2);
sensitibility[0] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] +
(ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] +
(ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] +
(ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3];
//******
//POS Y
//******
state1 = origiState;
state1.Position = new PointD(state1.Position.X,
ComMath.Normal(ComMath.Normal(state1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y));
this.SimulateModel(state1, origiInput, out output1);
state2 = origiState;
state2.Position = new PointD(state2.Position.X,
ComMath.Normal(ComMath.Normal(state2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y));
this.SimulateModel(state2, origiInput, out output2);
sensitibility[1] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] +
(ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] +
(ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] +
(ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3];
//******
//ORIENTATION X
//******
state1 = origiState;
state1.Orientation = new PointD(ComMath.Normal(ComMath.Normal(state1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY),
state1.Orientation.Y);
this.SimulateModel(state1, origiInput, out output1);
state2 = origiState;
state2.Orientation = new PointD(ComMath.Normal(ComMath.Normal(state2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY),
state2.Orientation.Y);
this.SimulateModel(state2, origiInput, out output2);
sensitibility[2] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] +
(ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] +
(ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] +
(ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3];
//******
//ORIENTATION Y
//******
state1 = origiState;
state1.Orientation = new PointD(state1.Orientation.X,
ComMath.Normal(ComMath.Normal(state1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY));
this.SimulateModel(state1, origiInput, out output1);
state2 = origiState;
state2.Orientation = new PointD(state2.Orientation.X,
ComMath.Normal(ComMath.Normal(state2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY));
this.SimulateModel(state2, origiInput, out output2);
sensitibility[3] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] +
(ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] +
(ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] +
(ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3];
//******
//LEFT SPEED
//******
input1 = origiInput;
input1.LeftSpeed = ComMath.Normal(ComMath.Normal(input1.LeftSpeed, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED);
this.SimulateModel(origiState, input1, out output1);
input2 = origiInput;
input2.LeftSpeed = ComMath.Normal(ComMath.Normal(input2.LeftSpeed, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED);
this.SimulateModel(origiState, input2, out output2);
sensitibility[4] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] +
(ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] +
(ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] +
(ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3];
//******
//RIGHT SPEED
//******
input1 = origiInput;
input1.RightSpeed = ComMath.Normal(ComMath.Normal(input1.RightSpeed, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED);
this.SimulateModel(origiState, input1, out output1);
input2 = origiInput;
input2.RightSpeed = ComMath.Normal(ComMath.Normal(input2.RightSpeed, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED);
this.SimulateModel(origiState, input2, out output2);
sensitibility[5] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] +
(ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] +
(ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] +
(ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3];
//******
//WHEEL ANGLE
//******
input1 = origiInput;
input1.Angle = ComMath.Normal(ComMath.Normal(input1.Angle, CarModelInput.MIN_ANGLE, CarModelInput.MAX_ANGLE,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelInput.MIN_ANGLE, CarModelInput.MAX_ANGLE);
this.SimulateModel(origiState, input1, out output1);
input2 = origiInput;
input2.Angle = ComMath.Normal(ComMath.Normal(input2.Angle, CarModelInput.MIN_ANGLE, CarModelInput.MAX_ANGLE,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2,
NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
CarModelInput.MIN_ANGLE, CarModelInput.MAX_ANGLE);
this.SimulateModel(origiState, input2, out output2);
sensitibility[6] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] +
(ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] +
(ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] +
(ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3];
this.state = origiState;
this.input = origiInput;
}
public void Simulate(CarModelState initialState, int simCount, out CarModelInput[] inputs, out CarModelState[] states)
{
inputs = new CarModelInput[simCount];
states = new CarModelState[simCount];
CarModelState state = initialState;
for (int i = 0; i < simCount; ++i)
{
SimulateOneStep(state, out inputs[i], out states[i]);
state = states[i];
}
}
public static void SimulateOneStep(MLPDll controller, IModelSimulator model, CarModelState state, out CarModelInput outInput, out CarModelState outState)
{
double[] inputs = new double[4];
inputs[0] = ComMath.Normal(state.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, POSITION_SCALE * MIN_NEURON_VALUE, POSITION_SCALE * MAX_NEURON_VALUE);
inputs[1] = ComMath.Normal(state.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, POSITION_SCALE * MIN_NEURON_VALUE, POSITION_SCALE * MAX_NEURON_VALUE);
inputs[2] = ComMath.Normal(state.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
inputs[3] = ComMath.Normal(state.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
double[] controllerOutputs = controller.Output(inputs);
if (INPUT_TYPE == inputType.wheelAngle)
{
outInput = new CarModelInput();
outInput.Angle = ComMath.Normal(controllerOutputs[0], MIN_NEURON_VALUE, MAX_NEURON_VALUE, CarModelInput.MIN_ANGLE, CarModelInput.MAX_ANGLE);
}
else if (INPUT_TYPE == inputType.wheelSpeed)
{
outInput = new CarModelInput(ComMath.Normal(controllerOutputs[0], MIN_NEURON_VALUE, MAX_NEURON_VALUE, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED),
ComMath.Normal(controllerOutputs[1], MIN_NEURON_VALUE, MAX_NEURON_VALUE, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED));
//********
//hatrafele tilos mennie
if (outInput.LeftSpeed < 0) outInput.LeftSpeed = 0;
if (outInput.RightSpeed < 0) outInput.RightSpeed = 0;
//********
}
model.SimulateModel(state, outInput, out outState);
}
public void TakeSample()
{
sampledCarState = model.state;
}
private double obstacleFieldError(CarModelState state)
{
double err = 0;
List<ObstacleState> obstacles = obstacleProvider.GetObstacleStates(1);
foreach (ObstacleState obst in obstacles)
{
double x = ComMath.Normal(state.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
double y = ComMath.Normal(state.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
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_Y, CarModelState.MAX_POS_Y, 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;
}
public CarModel(CarModelState state)
{
mainColor = Color.Gray;
init(state);
}
public void SimulateOneStep(CarModelState state, out CarModelInput outInput, out CarModelState outState)
{
NeuralController.SimulateOneStep(this.controller, this.model, state, out outInput, out outState);
}
public void Simulate(IModelSimulator modelsim, CarModelInput input, double timestep)
{
CarModel model = new CarModel(GetCarState());
model.SimulateModel(input, modelsim, timestep);
currentCarModelState = model.state;
}
//public NeuralController(IModelSimulator model, IObstaclePositionProvider obstacle, ICarPositionProvider start, IFinishPositionProvider finish, string filename)
//{
// this.model = model;
// controller = new MLPDll(filename);
// obstacleProvider = obstacle;
// carStateProvider = start;
// finishStateProvider = finish;
// trainingStopped = true;
// mu = 0.005;
//}
public Bitmap VisualizeObstacleField(int width, int height)
{
lock (this)
{
uint[] buf = new uint[width * height];
for (int x = 0; x < width; ++x)
for (int y = 0; y < height; ++y)
{
CarModelState state = new CarModelState();
state.Angle = 0;
state.Position = new PointD(ComMath.Normal(x, 0, width - 1, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
ComMath.Normal(y, 0, height - 1, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y));
double err = obstacleFieldError(state);
if (err > 255) err = 255;
buf[y * width + x] = 0x80000000 + ((uint)(err));
}
Bitmap bm = new Bitmap(width, height, PixelFormat.Format32bppArgb);
BitmapData bmData = bm.LockBits(new Rectangle(0, 0, width, height), ImageLockMode.ReadWrite, PixelFormat.Format32bppArgb);
int len = bmData.Width * bmData.Height;
unsafe
{
uint* cim = (uint*)bmData.Scan0.ToPointer();//direkt bitmap memoriaba rajzolunk, gyors
for (int i = 0; i < len; ++i)
{
cim[i] = buf[i];
}
}
bm.UnlockBits(bmData);
return bm;
}
}
//public CarModelState GetState(int index)
//{
// double[] ret = GetPosition(index);
// CarModelState cs = new CarModelState();
// cs.Angle = -ret[2];
// //cs.Position = new PointD(ComMath.Normal(ret[0], 0, im.Width, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X), ComMath.Normal(im.Height - ret[1], 0, im.Height, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y));
// return cs;
//}
public void TakeSample()
{
RunMarkerFinder();
sampledCarState = currentCarModelState;
}
public CarModel(CarModelState state, Color c)
{
mainColor = c;
init(state);
}
private void RunMarkerFinder()
{
if (mf != null)
{
Bitmap frame = camera.GetBitmap();
List<Shape> shapes = null;
shapes = mf.ProcessFrame(frame);
bool carFound = false;
bool finishFound = false;
// finish
currentObstacleStates = new List<ObstacleState>();
foreach (Shape s in shapes)
{
if ((s.index == 0) && (s.scale > 0.17) && (s.scale < 0.22))
{
currentCarModelState = new CarModelState(new PointD(ComMath.Normal(s.pos.X, 0, frame.Width, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
ComMath.Normal(s.pos.Y, 0, frame.Height, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)),
-Math.PI / 2 - s.rot);
carFound = true;
}
else if ((s.index == 1) && (s.scale > 0.16) && (s.scale < 0.20))
{
currentFinishState = new FinishState(new PointD(ComMath.Normal(s.pos.X, 0, frame.Width, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
ComMath.Normal(s.pos.Y, 0, frame.Height, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)),
-Math.PI / 2 - s.rot);
finishPredictor.AddPoint(currentFinishState.Position, currentFinishState.Orientation);
finishFound = true;
}
else
{
currentObstacleStates.Add(new ObstacleState(new PointD(ComMath.Normal(s.pos.X, 0, frame.Width, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
ComMath.Normal(s.pos.Y, 0, frame.Height, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)),
s.scale * 500));
//ide kell egy robusztus alg
//ha zaj jon, akkor ne vegye be
}
}
lock (obstacles)
{
List<ObstacleState> curObState = new List<ObstacleState>(currentObstacleStates);
foreach (ObstacleModel ops in obstacles)
{
List<PointD> list = ops.state.pp.PredictNextPositions(1);
ObstacleState osw =null;
PointD pd;
if (list != null) pd = list[0];
else pd = ops.state.pp.position;
double mindist = double.MaxValue;
foreach (ObstacleState os in curObState)
{
double dist = (os.pp.position.X - pd.X) * (os.pp.position.X - pd.X) + (os.pp.position.Y - pd.Y) * (os.pp.position.Y - pd.Y);
if (dist < mindist)
{
mindist = dist;
osw = os;
}
}
if (osw != null)
{
lock (ops)
{
ops.state.pp.AddNewPosition(osw.pp.position);
}
curObState.Remove(osw);
}
else
{
//a predikcioval leptetjuk tovabb
lock (ops)
{
ops.state.pp.AddNewPosition(pd);
}
}
}
foreach(ObstacleState os in curObState)
{
lock (obstacles)
{
ObstacleModel om = new ObstacleModel(os.pp.position, os.radius);
om.SetSelectedState(1, 0);
if (obstacles.Count < 1) obstacles.Add(om);
}
}
}
}
}
