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 Simulate(IModelSimulator modelsim, CarModelInput input, double timestep) { CarModel model = new CarModel(GetCarState()); model.SimulateModel(input, modelsim, timestep); currentCarModelState = model.state; }
public void Simulate(IModelSimulator modelsim, CarModelInput input, double timestep) { model.SimulateModel(input, modelsim, timestep); lock (obstacles) { foreach (ObstacleModel ops in obstacles) { lock (ops) { ops.state.pp.Simulate(timestep); } } } }
public NeuralController(IModelSimulator model, IObstaclePositionProvider obstacle, ICarPositionProvider start, IFinishPositionProvider finish) { this.model = model; if (INPUT_TYPE == inputType.wheelAngle) { controller = new MLPDll(new int[] { 20, 1 }, 4);//4 bemenet a state, 1 kimenet az input } else if (INPUT_TYPE == inputType.wheelSpeed) { controller = new MLPDll(new int[] { 75, 2 }, 4);//4 bemenet a state, 2 kimenet az input } obstacleProvider = obstacle; carStateProvider = start; finishStateProvider = finish; trainingStopped = true; mu = 0.005; }
//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 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); }
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 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(CarModelInput input, IModelSimulator simulator, double timeStep) { simulator.SimulateModel(this.state, input, timeStep, out this.state); }
public void SimulateModel(CarModelInput input, IModelSimulator simulator) { simulator.SimulateModel(this.state, input, out this.state); }