Пример #1
0
        public void Simulate(IModelSimulator modelsim, CarModelInput input, double timestep)
        {
            CarModel model = new CarModel(GetCarState());

            model.SimulateModel(input, modelsim, timestep);
            currentCarModelState = model.state;
        }
Пример #2
0
        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));
        }
Пример #3
0
        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);
        }
Пример #4
0
        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 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;
        }
Пример #6
0
        //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 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 void SimulateModel(CarModelState state, CarModelInput input, double timeStep, out CarModelState output)
 {
     double[] NNout;
     SimulateModel(state, input, timeStep, out output, out NNout);
 }
 public void SimulateModel(CarModelState state, CarModelInput input, out CarModelState output, out double[] NNOutput)
 {
     SimulateModel(state, input, CarModel.SIMULATION_TIME_STEP, out output, out NNOutput);
 }
Пример #11
0
        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();                               
        }
Пример #12
0
 public void SimulateModel(CarModelInput input, IModelSimulator simulator, double timeStep)
 {
     simulator.SimulateModel(this.state, input, timeStep, out this.state);
 }
Пример #13
0
        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);
        }
Пример #14
0
 public void SimulateModel(CarModelInput input, IModelSimulator simulator, double timeStep)
 {
     simulator.SimulateModel(this.state, input, timeStep, out this.state);
 }
Пример #15
0
        //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;
        }
Пример #16
0
        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);
        }
Пример #17
0
        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();
        }
Пример #18
0
        //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, 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));
        }
Пример #20
0
 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];
     }            
 }
Пример #21
0
 public void SimulateModel(CarModelInput input, IModelSimulator simulator)
 {
     simulator.SimulateModel(this.state, input, out this.state);
 }
Пример #22
0
 //matematikai modell
 public void SimulateModel(CarModelState state, CarModelInput input, out CarModelState output)
 {
     SimulateModel(state, input, CarModel.SIMULATION_TIME_STEP, out output);
 }
Пример #23
0
 public void SimulateOneStep(CarModelState state, out CarModelInput outInput, out CarModelState outState)
 {
     NeuralController.SimulateOneStep(this.controller, this.model, state, out outInput, out outState);
 }
Пример #24
0
        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 Simulate(IModelSimulator modelsim, CarModelInput input, double timestep)
 {
     CarModel model = new CarModel(GetCarState());
     model.SimulateModel(input, modelsim, timestep);
     currentCarModelState = model.state;
 }
Пример #26
0
 public void SimulateOneStep(CarModelState state, out CarModelInput outInput, out CarModelState outState)
 {
     NeuralController.SimulateOneStep(this.controller, this.model, state, out outInput, out outState);
 }
Пример #27
0
 public void SimulateModel(CarModelState state, CarModelInput input, double timeStep, out CarModelState output)
 {
     double[] NNout;
     SimulateModel(state, input, timeStep, out output, out NNout);
 }
 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);
             }
         }
     }
 }
Пример #29
0
 public void SimulateModel(CarModelInput input, IModelSimulator simulator)
 {            
     simulator.SimulateModel(this.state, input, out this.state);
 }