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);
}
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);
}
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;
}
protected override void OnPaint(PaintEventArgs pe)
{
Graphics g = pe.Graphics;
g.Clear(Color.White);
if (transform == null)
{
CalcTransform();
}
g.Transform = transform;
int w = (int)((CarModelState.MAX_POS_X - CarModelState.MIN_POS_X) / CarModel.MM_PER_PIXEL);
int h = (int)((CarModelState.MAX_POS_Y - CarModelState.MIN_POS_Y) / CarModel.MM_PER_PIXEL);
if (neuralController != null)
{
double orix = Math.Cos(carPositionProvider.GetCarState().Angle);
double oriy = Math.Sin(carPositionProvider.GetCarState().Angle);
PointF[] pp = new PointF[] { new PointF(0, 0), new PointF(Width - 1, Height - 1) };
itransform.TransformPoints(pp);
pp[0].X = (float)ComMath.Normal((pp[0].X - CarModel.OFFSET_X) * CarModel.MM_PER_PIXEL, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE);
pp[0].Y = (float)ComMath.Normal((pp[0].Y - CarModel.OFFSET_Y) * CarModel.MM_PER_PIXEL, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE);
pp[1].X = (float)ComMath.Normal((pp[1].X - CarModel.OFFSET_X) * CarModel.MM_PER_PIXEL, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE);
pp[1].Y = (float)ComMath.Normal((pp[1].Y - CarModel.OFFSET_Y) * CarModel.MM_PER_PIXEL, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE);
Bitmap background = neuralController.controller.Visualize(20, 20, new RectangleF(pp[0].X, pp[0].Y, pp[1].X - pp[0].X, pp[1].Y - pp[0].Y), 0, 1, new double[] { 0, 0, orix, oriy }, 0, -10, 10);
pp = new PointF[] { new PointF(0, 0), new PointF(Width - 1, Height - 1) };
itransform.TransformPoints(pp);
g.DrawImage(background, new RectangleF(pp[0].X, pp[0].Y, pp[1].X - pp[0].X, pp[1].Y - pp[0].Y), new RectangleF(0, 0, background.Width - 1, background.Height - 1), GraphicsUnit.Pixel);
}
if (cameraCarPos != null)
{
Image im = cameraCarPos.GetImage();
if (im != null)
{
g.DrawImage(im, new Rectangle(0, 0, w, h), new Rectangle(0, 0, im.Width, im.Height), GraphicsUnit.Pixel);
}
}
//g.DrawRectangle(new Pen(Color.Blue, 3), new Rectangle(0, 0, w, h));
if (obstacleProvider != null)
{
List <ObstacleModel> obstacles = obstacleProvider.GetObstacleModels(0);
foreach (ObstacleModel om in obstacles)
{
om.Render(g);
}
}
if (finishPositionProvider != null)
{
finishPositionProvider.GetFinishModel(0).Render(g);
}
if (trainingModels != null)
{
lock (trainingModels)
{
foreach (CarModel m in trainingModels)
{
if (m != null)
{
m.Render(g, 120, false);
}
}
}
}
if (carPositionProvider != null)
{
CarModel model = carPositionProvider.GetCarModel();
model.Render(g, 255, true);
if (recv)
{
g.FillEllipse(new SolidBrush(Color.Red), new Rectangle((int)(model.state.Position.X / CarModel.MM_PER_PIXEL + CarModel.OFFSET_X) - 2, (int)(model.state.Position.Y / CarModel.MM_PER_PIXEL + CarModel.OFFSET_Y) - 2, 4, 4));
recv = false;
}
}
// Calling the base class OnPaint
base.OnPaint(pe);
if (OnRefreshed != null)
{
OnRefreshed.Invoke();
}
}