Ejemplo n.º 1
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));
        }
Ejemplo n.º 2
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);
        }
Ejemplo n.º 3
0
        private static double[] obstacleFieldErrorGradient(IObstaclePositionProvider ops, GridCarModelState state, int time)
        {
            //C = sum((1/d(X) - 1/d(0))^2)
            //dC/dy_x =...
            //dC/dy_y =...
            double ksi     = 0.1;
            double disterr = 0;
            double orxerr  = 0;
            double oryerr  = 0;

            List <ObstacleState> obstacles = ops.GetObstacleStates(0);

            foreach (ObstacleState obst in obstacles)//cel az origo, tehat az origohoz relativak az akadalyok, origo felfele nez
            {
                double d = ComMath.Normal(Math.Sqrt(obst.pp.position.X * obst.pp.position.X + obst.pp.position.Y * obst.pp.position.Y), GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, 0, MAX_NEURON_VALUE);
                double a = ComMath.Normal(state.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, 0, MAX_NEURON_VALUE);

                double ang = Math.PI - (Math.Atan2(obst.pp.position.Y, obst.pp.position.X) + Math.PI) + state.TargetAngle - state.TargetFinishAngle;
                if (ang > Math.PI)
                {
                    ang -= 2 * Math.PI;
                }
                if (ang < -Math.PI)
                {
                    ang += 2 * Math.PI;
                }

                double AA       = -2 * d * Math.Cos(ang);
                double BB       = d * d;
                double obstdist = Math.Sqrt(a * a + BB + AA * a);
                double obstang  = state.TargetAngle + Math.Sign(ang) * Math.Acos((a * a + obstdist * obstdist - d * d) / (2 * a * obstdist));


                double r    = ComMath.Normal(obst.radius + CarModel.SHAFT_LENGTH / 2, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, 0, MAX_NEURON_VALUE);
                double dist = obstdist - r;
                if (dist <= 0.0001)
                {
                    dist = 0.0001;
                }
                double err = 1 / (2 * (dist * dist * dist));


                disterr += -(AA + 2 * a) * err;
                double angerr = (-2 * a * d * Math.Sin(ang)) * err;

                orxerr += -Math.Sin(state.TargetAngle) * angerr;
                oryerr += Math.Cos(state.TargetAngle) * angerr;
            }
            if (obstacles.Count > 0)
            {
                disterr /= obstacles.Count;
                orxerr  /= obstacles.Count;
                oryerr  /= obstacles.Count;
            }

            return(new double[] { -ksi * disterr, -ksi * orxerr, -ksi * oryerr });
        }
Ejemplo n.º 4
0
 public CameraObjectPositionProvider(PictureBox pb)
 {
     this.pb            = pb;
     camera             = new CameraDirectShow();
     camera.OnNewFrame += new OnNewFrameDelegate(camera_OnNewFrame);
     camera.Start();
     finishPredictor      = new PositionAndOrientationPredictor(30, 10);
     obstacles            = new List <ObstacleModel>();
     currentCarModelState = new CarModelState(new PointD(ComMath.Normal(0.05, 0, 1, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
                                                         ComMath.Normal(0.05, 0, 1, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)),
                                              new PointD(0, 1));
     currentFinishState = new FinishState(new PointD(ComMath.Normal(0.95, 0, 1, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
                                                     ComMath.Normal(0.95, 0, 1, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)),
                                          0.5 * Math.PI);
 }
Ejemplo n.º 5
0
        public static void SimulateOneStep(MLPDll controller, IGridModelSimulator model, GridCarModelState state, out GridCarModelInput outInput, out GridCarModelState outState)
        {
            double[] inputs = new double[5];
            inputs[0] = ComMath.Normal(state.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, 0, MAX_NEURON_VALUE);
            inputs[1] = ComMath.Normal(state.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
            inputs[2] = ComMath.Normal(state.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
            inputs[3] = ComMath.Normal(state.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
            inputs[4] = ComMath.Normal(state.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
            double[] controllerOutputs = controller.Output(inputs);


            outInput       = new GridCarModelInput();
            outInput.Angle = ComMath.Normal(controllerOutputs[0], MIN_NEURON_VALUE, MAX_NEURON_VALUE, GridCarModelInput.MIN_ANGLE, GridCarModelInput.MAX_ANGLE);

            model.SimulateModel(state, outInput, out outState);
        }
Ejemplo n.º 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");
        }
Ejemplo n.º 7
0
        private double obstacleFieldError(CarModelState state)
        {
            double err = 0;
            List <ObstacleState> obstacles = obstacleProvider.GetObstacleStates(1);

            foreach (ObstacleState obst in obstacles)
            {
                double x  = ComMath.Normal(state.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
                double y  = ComMath.Normal(state.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
                double x0 = ComMath.Normal(obst.pp.position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
                double y0 = ComMath.Normal(obst.pp.position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE);

                double dist = Math.Sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0));
                err += Math.Pow(1 / dist - 1 / obst.radius, 2);
            }

            return(err);
        }
Ejemplo n.º 8
0
        //public NeuralController(IModelSimulator model, IObstaclePositionProvider obstacle, ICarPositionProvider start, IFinishPositionProvider finish, string filename)
        //{
        //    this.model = model;
        //    controller = new MLPDll(filename);
        //    obstacleProvider = obstacle;
        //    carStateProvider = start;
        //    finishStateProvider = finish;
        //    trainingStopped = true;
        //    mu = 0.005;
        //}

        public Bitmap VisualizeObstacleField(int width, int height)
        {
            lock (this)
            {
                uint[] buf = new uint[width * height];

                for (int x = 0; x < width; ++x)
                {
                    for (int y = 0; y < height; ++y)
                    {
                        CarModelState state = new CarModelState();
                        state.Angle    = 0;
                        state.Position = new PointD(ComMath.Normal(x, 0, width - 1, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
                                                    ComMath.Normal(y, 0, height - 1, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y));

                        double err = obstacleFieldError(state);
                        if (err > 255)
                        {
                            err = 255;
                        }
                        buf[y * width + x] = 0x80000000 + ((uint)(err));
                    }
                }


                Bitmap     bm     = new Bitmap(width, height, PixelFormat.Format32bppArgb);
                BitmapData bmData = bm.LockBits(new Rectangle(0, 0, width, height), ImageLockMode.ReadWrite, PixelFormat.Format32bppArgb);

                int len = bmData.Width * bmData.Height;
                unsafe
                {
                    uint *cim = (uint *)bmData.Scan0.ToPointer();//direkt bitmap memoriaba rajzolunk, gyors
                    for (int i = 0; i < len; ++i)
                    {
                        cim[i] = buf[i];
                    }
                }

                bm.UnlockBits(bmData);
                return(bm);
            }
        }
Ejemplo n.º 9
0
        private double[] obstacleFieldErrorGradient(CarModelState state, int time)
        {
            //C = sum((1/d(X) - 1/d(0))^2)
            //dC/dy_x =...
            //dC/dy_y =...
            double ksi  = 0.0001;
            double errX = 0;
            double errY = 0;
            List <ObstacleState> obstacles = obstacleProvider.GetObstacleStates(time);

            foreach (ObstacleState obst in obstacles)
            {
                double x  = ComMath.Normal(state.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
                double y  = ComMath.Normal(state.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
                double x0 = ComMath.Normal(obst.pp.position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
                double y0 = ComMath.Normal(obst.pp.position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE);


                double r = ComMath.Normal(obst.radius + CarModel.SHAFT_LENGTH / 2, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE);

                double dist = Math.Sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0)) - r;
                if (dist <= 0.0001)
                {
                    dist = 0.0001;
                }
                double err = (1 / (dist * dist * dist));
                errX += err * (x - x0);
                errY += err * (y - y0);
            }
            if (obstacles.Count > 0)
            {
                errX /= obstacles.Count;
                errY /= obstacles.Count;
            }
            return(new double[] { ksi *errX, ksi *errY, 0, 0 });
        }
Ejemplo n.º 10
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;
        }
Ejemplo n.º 11
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);
        }
Ejemplo n.º 12
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();
        }
        public void CalcErrorSensibility(double[] errors, out double[] sensitibility)//errors: distance error, orientation x,y error, finish orientation x,y error
        {
            GridCarModelState output1, output2, state1, state2, origiState = this.state;
            GridCarModelInput input1, input2, origiInput = this.input;
            double            DIFF_C = 0.001;

            sensitibility = new double[6];

            //******
            //DIST
            //******

            state1            = origiState;
            state1.TargetDist = ComMath.Normal(ComMath.Normal(state1.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST,
                                                              NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2,
                                               NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
                                               GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST);

            this.SimulateModel(state1, origiInput, out output1);
            state2            = origiState;
            state2.TargetDist = ComMath.Normal(ComMath.Normal(state2.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST,
                                                              NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2,
                                               NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
                                               GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST);
            this.SimulateModel(state2, origiInput, out output2);
            sensitibility[0] = (ComMath.Normal(output2.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] +
                               (ComMath.Normal(output2.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] +
                               (ComMath.Normal(output2.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] +
                               (ComMath.Normal(output2.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3] +
                               (ComMath.Normal(output2.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[4];


            //******
            //ORIENTATION X
            //******

            state1 = origiState;
            state1.TargetOrientation = new PointD(ComMath.Normal(ComMath.Normal(state1.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY,
                                                                                NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2,
                                                                 NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
                                                                 GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY),
                                                  state1.TargetOrientation.Y);
            this.SimulateModel(state1, origiInput, out output1);
            state2 = origiState;
            state2.TargetOrientation = new PointD(ComMath.Normal(ComMath.Normal(state2.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY,
                                                                                NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2,
                                                                 NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
                                                                 GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY),
                                                  state2.TargetOrientation.Y);
            this.SimulateModel(state2, origiInput, out output2);
            sensitibility[1] = (ComMath.Normal(output2.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] +
                               (ComMath.Normal(output2.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] +
                               (ComMath.Normal(output2.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] +
                               (ComMath.Normal(output2.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3] +
                               (ComMath.Normal(output2.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[4];
            //******
            //ORIENTATION Y
            //******

            state1 = origiState;
            state1.TargetOrientation = new PointD(state1.TargetOrientation.X,
                                                  ComMath.Normal(ComMath.Normal(state1.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY,
                                                                                NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2,
                                                                 NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
                                                                 GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY));
            this.SimulateModel(state1, origiInput, out output1);
            state2 = origiState;
            state2.TargetOrientation = new PointD(state2.TargetOrientation.X,
                                                  ComMath.Normal(ComMath.Normal(state2.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY,
                                                                                NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2,
                                                                 NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
                                                                 GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY));
            this.SimulateModel(state2, origiInput, out output2);
            sensitibility[2] = (ComMath.Normal(output2.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] +
                               (ComMath.Normal(output2.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] +
                               (ComMath.Normal(output2.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] +
                               (ComMath.Normal(output2.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3] +
                               (ComMath.Normal(output2.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[4];

            //******
            //FINISH ORIENTATION X
            //******

            state1 = origiState;
            state1.TargetFinishOrientation = new PointD(ComMath.Normal(ComMath.Normal(state1.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY,
                                                                                      NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2,
                                                                       NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
                                                                       GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY),
                                                        state1.TargetFinishOrientation.Y);
            this.SimulateModel(state1, origiInput, out output1);
            state2 = origiState;
            state2.TargetFinishOrientation = new PointD(ComMath.Normal(ComMath.Normal(state2.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY,
                                                                                      NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2,
                                                                       NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
                                                                       GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY),
                                                        state2.TargetFinishOrientation.Y);
            this.SimulateModel(state2, origiInput, out output2);
            sensitibility[3] = (ComMath.Normal(output2.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] +
                               (ComMath.Normal(output2.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] +
                               (ComMath.Normal(output2.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] +
                               (ComMath.Normal(output2.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3] +
                               (ComMath.Normal(output2.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[4];
            //******
            //FINISH ORIENTATION Y
            //******

            state1 = origiState;
            state1.TargetFinishOrientation = new PointD(state1.TargetFinishOrientation.X,
                                                        ComMath.Normal(ComMath.Normal(state1.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY,
                                                                                      NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2,
                                                                       NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
                                                                       GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY));
            this.SimulateModel(state1, origiInput, out output1);
            state2 = origiState;
            state2.TargetFinishOrientation = new PointD(state2.TargetFinishOrientation.X,
                                                        ComMath.Normal(ComMath.Normal(state2.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY,
                                                                                      NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2,
                                                                       NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
                                                                       GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY));
            this.SimulateModel(state2, origiInput, out output2);
            sensitibility[4] = (ComMath.Normal(output2.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] +
                               (ComMath.Normal(output2.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] +
                               (ComMath.Normal(output2.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] +
                               (ComMath.Normal(output2.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3] +
                               (ComMath.Normal(output2.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[4];

            //******
            //ANGLE
            //******

            input1       = origiInput;
            input1.Angle = ComMath.Normal(ComMath.Normal(input1.Angle, GridCarModelInput.MIN_ANGLE, GridCarModelInput.MAX_ANGLE,
                                                         NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2,
                                          NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
                                          GridCarModelInput.MIN_ANGLE, GridCarModelInput.MAX_ANGLE);
            this.SimulateModel(origiState, input1, out output1);
            input2       = origiInput;
            input2.Angle = ComMath.Normal(ComMath.Normal(input2.Angle, GridCarModelInput.MIN_ANGLE, GridCarModelInput.MAX_ANGLE,
                                                         NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2,
                                          NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE,
                                          GridCarModelInput.MIN_ANGLE, GridCarModelInput.MAX_ANGLE);
            this.SimulateModel(origiState, input2, out output2);
            sensitibility[5] = (ComMath.Normal(output2.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] +
                               (ComMath.Normal(output2.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] +
                               (ComMath.Normal(output2.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] +
                               (ComMath.Normal(output2.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3] +
                               (ComMath.Normal(output2.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) -
                                ComMath.Normal(output1.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[4];


            this.state = origiState;
            this.input = origiInput;
        }
Ejemplo n.º 14
0
        private static double TrainOneEpoch(MLPDll controller, IGridModelSimulator model, ICarPositionProvider cps, IFinishPositionProvider fps, IObstaclePositionProvider ops, double mu, int maxSimCount, out double SumSimCount, out List <GridCarModelState> innerStates)
        {
            double sumSimCount = 0;
            double error       = 0;

            innerStates = new List <GridCarModelState>();
            List <double> deltaws = new List <double>();

            MLPDll[] controllers         = new MLPDll[maxSimCount];
            IGridModelSimulator[] models = new IGridModelSimulator[maxSimCount];

            GridCarModelState state = GridCarModelState.FromCarModelState(cps.GetCarState());
            GridCarModelInput input = new GridCarModelInput();


            //kimenet kiszamitasa
            int               simCount             = 0;
            List <double[]>   singleErrors         = new List <double[]>();
            List <double[]>   regularizationErrors = new List <double[]>();
            GridCarModelState laststate;
            bool              earlyStop;

            do
            {
                if (simCount == 0)
                {
                    controllers[simCount] = new MLPDll(controller);               //lemasoljuk
                }
                else
                {
                    controllers[simCount] = new MLPDll(controllers[simCount - 1]);
                }
                models[simCount] = model.Clone();//a modellt is

                laststate = state;
                GridNeuralController.SimulateOneStep(controllers[simCount], models[simCount], state, out input, out state);//vegigszimulaljuk a simCount darab controlleren es modellen
                innerStates.Add(state);

                //kozbulso hibak kiszamitasa, itt csak az akadalyoktol valo tavolsag "hibajat" vesszuk figyelembe, irany nem szamit -> hibaja 0
                regularizationErrors.Add(obstacleFieldErrorGradient(ops, state, simCount));

                //minden pont celtol vett tavolsaga
                double disterror        = ComMath.Normal(state.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, 0, 1);
                double orientationerror = disterror;
                if (orientationerror < 0.2)
                {
                    orientationerror = 0;
                }
                double finishorientationerror = disterror;
                if (finishorientationerror > 0.05)
                {
                    finishorientationerror = 0;
                }
                else
                {
                    finishorientationerror = 1;
                }
                double finishX = Math.Cos(Math.PI - fps.GetFinishState(simCount).Angle);
                double finishY = Math.Sin(Math.PI - fps.GetFinishState(simCount).Angle);

                singleErrors.Add(new double[] { -disterror * MAX_NEURON_VALUE,
                                                orientationerror * ComMath.Normal(1 - state.TargetOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE),
                                                orientationerror * ComMath.Normal(0 - state.TargetOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE),
                                                finishorientationerror * ComMath.Normal(finishX - state.TargetFinishOrientation.X, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE),
                                                finishorientationerror * ComMath.Normal(finishY - state.TargetFinishOrientation.Y, GridCarModelState.MIN_OR_XY, GridCarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE) }
                                 );
                ++simCount;

                earlyStop = false;
                if (simCount > 3)
                {
                    double[] err1 = singleErrors[simCount - 1];
                    double[] err2 = singleErrors[simCount - 2];
                    double[] err3 = singleErrors[simCount - 3];
                    double   error1, error2, error3;
                    error1 = error2 = error3 = 0;
                    for (int i = 0; i < 1; i++)//err1.Length
                    {
                        error1 += err1[i] * err1[i];
                        error2 += err2[i] * err2[i];
                        error3 += err3[i] * err3[i];
                    }
                    earlyStop = ((error1 > error2) && (error3 > error2));

                    if (earlyStop)
                    {
                        //utolso elemet toroljuk
                        singleErrors.RemoveAt(singleErrors.Count - 1);
                        regularizationErrors.RemoveAt(regularizationErrors.Count - 1);
                        innerStates.RemoveAt(innerStates.Count - 1);
                        --simCount;
                    }
                }
            }while ((simCount < maxSimCount) && !earlyStop);



            double[] errors = singleErrors[singleErrors.Count - 1];

            sumSimCount += simCount;

            //hibavisszaterjesztes
            for (int i = simCount - 1; i >= 0; --i)
            {
                double[] sensitibility;
                models[i].CalcErrorSensibility(errors, out sensitibility);

                double[] inputSensitibility;


                inputSensitibility    = new double[1];
                inputSensitibility[0] = sensitibility[5];


                double[] sensitibility2;

                controllers[i].SetOutputError(inputSensitibility);
                controllers[i].Backpropagate();
                controllers[i].CalculateDeltaWeights();
                sensitibility2 = controllers[i].SensitibilityD();



                errors[0] = (sensitibility[0] + sensitibility2[0] + 0.1 * singleErrors[i][0]);
                errors[1] = (sensitibility[1] + sensitibility2[1] + 0 * singleErrors[i][1]);
                errors[2] = (sensitibility[2] + sensitibility2[2] + 0 * singleErrors[i][2]);
                errors[3] = (sensitibility[3] + sensitibility2[3] + singleErrors[i][3]);
                errors[4] = (sensitibility[4] + sensitibility2[4] + singleErrors[i][4]);

                //regularizaciobol szarmazo hiba hozzaadasa
                errors[0] += regularizationErrors[i][0];
                errors[1] += regularizationErrors[i][1];
                errors[2] += regularizationErrors[i][2];
            }

            controller.ClearDeltaWeights();
            //sulymodositasok osszegzese
            for (int i2 = 0; i2 < simCount; ++i2)
            {
                controller.AddDeltaWeights(controllers[i2]);
            }
            float maxdw = controller.MaxDeltaWeight();

            //if (maxdw < 50) maxdw = 50;

            controller.ChangeWeights(mu / maxdw);

            ////sulymodositasok osszegzese
            //for (int i2 = 0; i2 < simCount; ++i2) //simCount
            //{

            //    int count = 0;
            //    for (int i = 1; i < controllers[i2]; ++i)
            //    {
            //        foreach (INeuron n in controllers[i2].mlp[i])
            //        {
            //            foreach (NeuronInput ni in ((Neuron)n).inputs)
            //            {
            //                if (deltaws.Count <= count) deltaws.Add(ni.deltaw);
            //                else deltaws[count] += ni.deltaw;
            //                ++count;
            //            }
            //        }
            //    }
            //}

            ////legnagyobb sulymodositas ertekenek meghatarozasa, majd ezzel normalas
            //double maxdw = 1;

            //foreach (double dw in deltaws)
            //{
            //    if (Math.Abs(dw) > maxdw) maxdw = Math.Abs(dw);
            //}

            //if (maxdw < 50) maxdw = 50;

            ////sulymodositasok ervenyre juttatasa a controllerben
            //int count2 = 0;

            //for (int i = 1; i < controller.mlp.Count; ++i)
            //{
            //    foreach (INeuron n in controller.mlp[i])
            //    {
            //        foreach (NeuronInput ni in ((Neuron)n).inputs)
            //        {
            //            ni.w += mu * deltaws[count2] / maxdw;

            //            ++count2;
            //        }
            //    }
            //}


            SumSimCount = sumSimCount;
            return(error);
        }
Ejemplo n.º 15
0
        private void RunMarkerFinder()
        {
            if (mf != null)
            {
                Bitmap       frame  = camera.GetBitmap();
                List <Shape> shapes = null;

                shapes = mf.ProcessFrame(frame);

                bool carFound    = false;
                bool finishFound = false;

                // finish
                currentObstacleStates = new List <ObstacleState>();
                foreach (Shape s in shapes)
                {
                    if ((s.index == 0) && (s.scale > 0.17) && (s.scale < 0.22))
                    {
                        currentCarModelState = new CarModelState(new PointD(ComMath.Normal(s.pos.X, 0, frame.Width, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
                                                                            ComMath.Normal(s.pos.Y, 0, frame.Height, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)),
                                                                 -Math.PI / 2 - s.rot);
                        carFound = true;
                    }
                    else if ((s.index == 1) && (s.scale > 0.16) && (s.scale < 0.20))
                    {
                        currentFinishState = new FinishState(new PointD(ComMath.Normal(s.pos.X, 0, frame.Width, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
                                                                        ComMath.Normal(s.pos.Y, 0, frame.Height, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)),
                                                             -Math.PI / 2 - s.rot);
                        finishPredictor.AddPoint(currentFinishState.Position, currentFinishState.Orientation);
                        finishFound = true;
                    }
                    else
                    {
                        currentObstacleStates.Add(new ObstacleState(new PointD(ComMath.Normal(s.pos.X, 0, frame.Width, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
                                                                               ComMath.Normal(s.pos.Y, 0, frame.Height, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)),
                                                                    s.scale * 500));
                        //ide kell egy robusztus alg
                        //ha zaj jon, akkor ne vegye be
                    }
                }

                lock (obstacles)
                {
                    List <ObstacleState> curObState = new List <ObstacleState>(currentObstacleStates);
                    foreach (ObstacleModel ops in obstacles)
                    {
                        List <PointD> list = ops.state.pp.PredictNextPositions(1);
                        ObstacleState osw  = null;

                        PointD pd;
                        if (list != null)
                        {
                            pd = list[0];
                        }
                        else
                        {
                            pd = ops.state.pp.position;
                        }

                        double mindist = double.MaxValue;
                        foreach (ObstacleState os in curObState)
                        {
                            double dist = (os.pp.position.X - pd.X) * (os.pp.position.X - pd.X) + (os.pp.position.Y - pd.Y) * (os.pp.position.Y - pd.Y);
                            if (dist < mindist)
                            {
                                mindist = dist;
                                osw     = os;
                            }
                        }

                        if (osw != null)
                        {
                            lock (ops)
                            {
                                ops.state.pp.AddNewPosition(osw.pp.position);
                            }
                            curObState.Remove(osw);
                        }
                        else
                        {
                            //a predikcioval leptetjuk tovabb
                            lock (ops)
                            {
                                ops.state.pp.AddNewPosition(pd);
                            }
                        }
                    }

                    foreach (ObstacleState os in curObState)
                    {
                        lock (obstacles)
                        {
                            ObstacleModel om = new ObstacleModel(os.pp.position, os.radius);
                            om.SetSelectedState(1, 0);
                            if (obstacles.Count < 1)
                            {
                                obstacles.Add(om);
                            }
                        }
                    }
                }
            }
        }
Ejemplo n.º 16
0
        protected override void OnPaint(PaintEventArgs pe)
        {
            Graphics g = pe.Graphics;

            g.Clear(Color.White);
            if (transform == null)
            {
                CalcTransform();
            }
            g.Transform = transform;


            int w = (int)((CarModelState.MAX_POS_X - CarModelState.MIN_POS_X) / CarModel.MM_PER_PIXEL);
            int h = (int)((CarModelState.MAX_POS_Y - CarModelState.MIN_POS_Y) / CarModel.MM_PER_PIXEL);


            if (neuralController != null)
            {
                double orix = Math.Cos(carPositionProvider.GetCarState().Angle);
                double oriy = Math.Sin(carPositionProvider.GetCarState().Angle);

                PointF[] pp = new PointF[] { new PointF(0, 0), new PointF(Width - 1, Height - 1) };
                itransform.TransformPoints(pp);

                pp[0].X = (float)ComMath.Normal((pp[0].X - CarModel.OFFSET_X) * CarModel.MM_PER_PIXEL, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE);
                pp[0].Y = (float)ComMath.Normal((pp[0].Y - CarModel.OFFSET_Y) * CarModel.MM_PER_PIXEL, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE);
                pp[1].X = (float)ComMath.Normal((pp[1].X - CarModel.OFFSET_X) * CarModel.MM_PER_PIXEL, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE);
                pp[1].Y = (float)ComMath.Normal((pp[1].Y - CarModel.OFFSET_Y) * CarModel.MM_PER_PIXEL, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE);
                Bitmap background = neuralController.controller.Visualize(20, 20, new RectangleF(pp[0].X, pp[0].Y, pp[1].X - pp[0].X, pp[1].Y - pp[0].Y), 0, 1, new double[] { 0, 0, orix, oriy }, 0, -10, 10);


                pp = new PointF[] { new PointF(0, 0), new PointF(Width - 1, Height - 1) };
                itransform.TransformPoints(pp);

                g.DrawImage(background, new RectangleF(pp[0].X, pp[0].Y, pp[1].X - pp[0].X, pp[1].Y - pp[0].Y), new RectangleF(0, 0, background.Width - 1, background.Height - 1), GraphicsUnit.Pixel);
            }


            if (cameraCarPos != null)
            {
                Image im = cameraCarPos.GetImage();


                if (im != null)
                {
                    g.DrawImage(im, new Rectangle(0, 0, w, h), new Rectangle(0, 0, im.Width, im.Height), GraphicsUnit.Pixel);
                }
            }
            //g.DrawRectangle(new Pen(Color.Blue, 3), new Rectangle(0, 0, w, h));


            if (obstacleProvider != null)
            {
                List <ObstacleModel> obstacles = obstacleProvider.GetObstacleModels(0);
                foreach (ObstacleModel om in obstacles)
                {
                    om.Render(g);
                }
            }

            if (finishPositionProvider != null)
            {
                finishPositionProvider.GetFinishModel(0).Render(g);
            }


            if (trainingModels != null)
            {
                lock (trainingModels)
                {
                    foreach (CarModel m in trainingModels)
                    {
                        if (m != null)
                        {
                            m.Render(g, 120, false);
                        }
                    }
                }
            }

            if (carPositionProvider != null)
            {
                CarModel model = carPositionProvider.GetCarModel();
                model.Render(g, 255, true);
                if (recv)
                {
                    g.FillEllipse(new SolidBrush(Color.Red), new Rectangle((int)(model.state.Position.X / CarModel.MM_PER_PIXEL + CarModel.OFFSET_X) - 2, (int)(model.state.Position.Y / CarModel.MM_PER_PIXEL + CarModel.OFFSET_Y) - 2, 4, 4));
                    recv = false;
                }
            }

            // Calling the base class OnPaint
            base.OnPaint(pe);
            if (OnRefreshed != null)
            {
                OnRefreshed.Invoke();
            }
        }