private void init(CarModelState state) { this.state = state; int len = (int)(SHAFT_LENGTH / MM_PER_PIXEL / 2); graphicCarModelWheelLeft = new PointF[] { new PointF(-5, -len - 3), new PointF(5, -len - 3), new PointF(5, -len + 2), new PointF(-5, -len + 2) }; graphicCarModelWheelRight = new PointF[] { new PointF(-5, len - 2), new PointF(5, len - 2), new PointF(5, len + 3), new PointF(-5, len + 3) }; graphicCarModelBody = new PointF[] { new PointF(-1, -len + 2), new PointF(-len * 2, -len + 2), new PointF(-len * 2, len - 2), new PointF(-1, len - 2) }; }
public void Simulate(IModelSimulator modelsim, CarModelInput input, double timestep) { CarModel model = new CarModel(GetCarState()); model.SimulateModel(input, modelsim, timestep); currentCarModelState = model.state; }
public static void SimulateOneStep(MLPDll controller, IModelSimulator model, CarModelState state, out CarModelInput outInput, out CarModelState outState) { double[] inputs = new double[4]; inputs[0] = ComMath.Normal(state.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, POSITION_SCALE * MIN_NEURON_VALUE, POSITION_SCALE * MAX_NEURON_VALUE); inputs[1] = ComMath.Normal(state.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, POSITION_SCALE * MIN_NEURON_VALUE, POSITION_SCALE * MAX_NEURON_VALUE); inputs[2] = ComMath.Normal(state.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE); inputs[3] = ComMath.Normal(state.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE); double[] controllerOutputs = controller.Output(inputs); if (INPUT_TYPE == inputType.wheelAngle) { outInput = new CarModelInput(); outInput.Angle = ComMath.Normal(controllerOutputs[0], MIN_NEURON_VALUE, MAX_NEURON_VALUE, CarModelInput.MIN_ANGLE, CarModelInput.MAX_ANGLE); } else if (INPUT_TYPE == inputType.wheelSpeed) { outInput = new CarModelInput(ComMath.Normal(controllerOutputs[0], MIN_NEURON_VALUE, MAX_NEURON_VALUE, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED), ComMath.Normal(controllerOutputs[1], MIN_NEURON_VALUE, MAX_NEURON_VALUE, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED)); //******** //hatrafele tilos mennie if (outInput.LeftSpeed < 0) { outInput.LeftSpeed = 0; } if (outInput.RightSpeed < 0) { outInput.RightSpeed = 0; } //******** } model.SimulateModel(state, outInput, out outState); }
public void SimulateModel(CarModelState state, CarModelInput input, double timeStep, out CarModelState output, out double[] NNOutput) { double[] inputs = new double[7]; if (NeuralController.INPUT_TYPE == inputType.wheelAngle) { inputs[6] = ComMath.Normal(input.Angle, CarModelInput.MIN_ANGLE, CarModelInput.MAX_ANGLE, MIN_NEURON_VALUE, MAX_NEURON_VALUE); } else if (NeuralController.INPUT_TYPE == inputType.wheelSpeed) { inputs[4] = ComMath.Normal(input.LeftSpeed, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED, MIN_NEURON_VALUE, MAX_NEURON_VALUE); inputs[5] = ComMath.Normal(input.RightSpeed, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED, MIN_NEURON_VALUE, MAX_NEURON_VALUE); } inputs[0] = ComMath.Normal(state.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE); inputs[1] = ComMath.Normal(state.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE); inputs[2] = ComMath.Normal(state.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE); inputs[3] = ComMath.Normal(state.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE); NNOutput = mlp.Output(inputs); double X = ComMath.Normal(NNOutput[0], MIN_NEURON_VALUE, MAX_NEURON_VALUE, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X); double Y = ComMath.Normal(NNOutput[1], MIN_NEURON_VALUE, MAX_NEURON_VALUE, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y); double oX = ComMath.Normal(NNOutput[2], MIN_NEURON_VALUE, MAX_NEURON_VALUE, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY); double oY = ComMath.Normal(NNOutput[3], MIN_NEURON_VALUE, MAX_NEURON_VALUE, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY); output = new CarModelState(new PointD(X, Y), new PointD(oX, oY)); }
public static CarModelState ToCarModelState(GridCarModelState s) { CarModelState cms = new CarModelState(); cms.Position = new PointD(Math.Cos(s.TargetAngle - s.TargetFinishAngle) * s.TargetDist, Math.Sin(s.TargetAngle - s.TargetFinishAngle) * s.TargetDist); cms.Angle = Math.PI - s.TargetFinishAngle; return cms; }
public SimulationModeItemManager(ContextMenuStrip contextMenuStrip, CarModelGraphicControl graphicControl) { model = new CarModel(new CarModelState(new PointD(ComMath.Normal(0, 0, 1, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X), ComMath.Normal(0, 0, 1, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)), new PointD(0, 1))); finish = new FinishModel(new PointD(ComMath.Normal(0.5, 0, 1, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X), ComMath.Normal(0.5, 0, 1, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)), 0); obstacles = new List<ObstacleModel>(); dragables = new List<IDragable>(); sampledCarState = model.state; dragables.Add(model); dragables.Add(finish); dragged = null; state = State.NONE; this.contextMenuStrip = contextMenuStrip; this.graphicControl = graphicControl; contextMenuStrip.Items[0].Click += new EventHandler(newObstacle); contextMenuStrip.Items[1].Click += new EventHandler(deleteObstacle); }
public static CarModelState ToCarModelState(GridCarModelState s) { CarModelState cms = new CarModelState(); cms.Position = new PointD(Math.Cos(s.TargetAngle - s.TargetFinishAngle) * s.TargetDist, Math.Sin(s.TargetAngle - s.TargetFinishAngle) * s.TargetDist); cms.Angle = Math.PI - s.TargetFinishAngle; return(cms); }
public static GridCarModelState FromCarModelState(CarModelState s) { GridCarModelState gcms = new GridCarModelState(); gcms.TargetFinishAngle = Math.PI-s.Angle; gcms.TargetAngle = Math.Atan2(s.Position.Y, s.Position.X) + gcms.TargetFinishAngle; gcms.TargetDist = Math.Sqrt(s.Position.Y * s.Position.Y + s.Position.X * s.Position.X); return gcms; }
public static GridCarModelState FromCarModelState(CarModelState s) { GridCarModelState gcms = new GridCarModelState(); gcms.TargetFinishAngle = Math.PI - s.Angle; gcms.TargetAngle = Math.Atan2(s.Position.Y, s.Position.X) + gcms.TargetFinishAngle; gcms.TargetDist = Math.Sqrt(s.Position.Y * s.Position.Y + s.Position.X * s.Position.X); return(gcms); }
public void Simulate(CarModelState initialState, int simCount, out CarModelInput[] inputs, out CarModelState[] states) { inputs = new CarModelInput[simCount]; states = new CarModelState[simCount]; CarModelState state = initialState; for (int i = 0; i < simCount; ++i) { SimulateOneStep(state, out inputs[i], out states[i]); state = states[i]; } }
public CameraObjectPositionProvider(PictureBox pb) { this.pb = pb; camera = new CameraDirectShow(); camera.OnNewFrame += new OnNewFrameDelegate(camera_OnNewFrame); camera.Start(); finishPredictor = new PositionAndOrientationPredictor(30, 10); obstacles = new List <ObstacleModel>(); currentCarModelState = new CarModelState(new PointD(ComMath.Normal(0.05, 0, 1, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X), ComMath.Normal(0.05, 0, 1, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)), new PointD(0, 1)); currentFinishState = new FinishState(new PointD(ComMath.Normal(0.95, 0, 1, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X), ComMath.Normal(0.95, 0, 1, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)), 0.5 * Math.PI); }
public CameraObjectPositionProvider(PictureBox pb) { this.pb = pb; camera = new CameraDirectShow(); camera.OnNewFrame += new OnNewFrameDelegate(camera_OnNewFrame); camera.Start(); finishPredictor = new PositionAndOrientationPredictor(30, 10); obstacles = new List<ObstacleModel>(); currentCarModelState = new CarModelState(new PointD(ComMath.Normal(0.05, 0, 1, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X), ComMath.Normal(0.05, 0, 1, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)), new PointD(0, 1)); currentFinishState = new FinishState(new PointD(ComMath.Normal(0.95, 0, 1, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X), ComMath.Normal(0.95, 0, 1, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)), 0.5 * Math.PI); }
//public NeuralModelSimulator(String filename) //{ // mlp = new MLPDll(filename); //} public void Train(IModelSimulator sourceSimulator, double treshold) { Random r = new Random(); double mu = 0.0001; long count = 0; double errors = 0, errors2 = double.MaxValue; double[] error = new double[4]; do { for (int i2 = 0; i2 < EPOCH_COUNT; ++i2) { double angle = r.NextDouble() * 2 * Math.PI;//veletlen szog CarModelState carstate = new CarModelState(new PointD(ComMath.Normal(r.NextDouble(), 0, 1, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X), ComMath.Normal(r.NextDouble(), 0, 1, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)), new PointD(ComMath.Normal(Math.Cos(angle), -1, 1, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY), ComMath.Normal(Math.Sin(angle), -1, 1, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY))); CarModelInput carinput = new CarModelInput(); //= new CarModelInput(ComMath.Normal(r.NextDouble(), 0, 1, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED), // ComMath.Normal(r.NextDouble(), 0, 1, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED)); carinput.Angle = ComMath.Normal(r.NextDouble(), 0, 1, CarModelInput.MIN_ANGLE, CarModelInput.MAX_ANGLE); CarModelState state, state2; double[] output; sourceSimulator.SimulateModel(carstate, carinput, out state); this.SimulateModel(carstate, carinput, out state2, out output); error = new double[4]; error[0] = -output[0] + ComMath.Normal(state.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE); error[1] = -output[1] + ComMath.Normal(state.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE); error[2] = -output[2] + ComMath.Normal(state.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE); error[3] = -output[3] + ComMath.Normal(state.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE); count++; mlp.Train(mu, error); errors += error[0] * error[0] + error[1] * error[1] + error[2] * error[2] + error[3] * error[3]; } errors /= EPOCH_COUNT; //if (errors2 < errors) mu *= 0.75; errors2 = errors; System.Console.WriteLine(errors.ToString()); } while (errors > treshold); // mlp.SaveNN("neuralmodel.mlp"); }
public void SimulateModel(CarModelState state, CarModelInput input, double timeStep, out CarModelState output) { this.state = state; this.input = input; CarModelState state2 = state; double dAngle = (input.LeftSpeed - input.RightSpeed) * timeStep / CarModel.SHAFT_LENGTH; double lamda = 1; if (dAngle != 0) lamda = 2 / dAngle * Math.Sin(dAngle / 2); double vectLength = (input.RightSpeed + input.LeftSpeed) / 2 * timeStep * lamda; state2.Angle += dAngle; PointD p = new PointD((state.Position.X + vectLength * Math.Cos(state2.Angle - dAngle / 2)), (state.Position.Y + vectLength * Math.Sin(state2.Angle - dAngle / 2))); state2.Position = p; output = state2; }
private double obstacleFieldError(CarModelState state) { double err = 0; List <ObstacleState> obstacles = obstacleProvider.GetObstacleStates(1); foreach (ObstacleState obst in obstacles) { double x = ComMath.Normal(state.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE); double y = ComMath.Normal(state.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE); double x0 = ComMath.Normal(obst.pp.position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE); double y0 = ComMath.Normal(obst.pp.position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE); double dist = Math.Sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0)); err += Math.Pow(1 / dist - 1 / obst.radius, 2); } return(err); }
//public NeuralController(IModelSimulator model, IObstaclePositionProvider obstacle, ICarPositionProvider start, IFinishPositionProvider finish, string filename) //{ // this.model = model; // controller = new MLPDll(filename); // obstacleProvider = obstacle; // carStateProvider = start; // finishStateProvider = finish; // trainingStopped = true; // mu = 0.005; //} public Bitmap VisualizeObstacleField(int width, int height) { lock (this) { uint[] buf = new uint[width * height]; for (int x = 0; x < width; ++x) { for (int y = 0; y < height; ++y) { CarModelState state = new CarModelState(); state.Angle = 0; state.Position = new PointD(ComMath.Normal(x, 0, width - 1, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X), ComMath.Normal(y, 0, height - 1, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)); double err = obstacleFieldError(state); if (err > 255) { err = 255; } buf[y * width + x] = 0x80000000 + ((uint)(err)); } } Bitmap bm = new Bitmap(width, height, PixelFormat.Format32bppArgb); BitmapData bmData = bm.LockBits(new Rectangle(0, 0, width, height), ImageLockMode.ReadWrite, PixelFormat.Format32bppArgb); int len = bmData.Width * bmData.Height; unsafe { uint *cim = (uint *)bmData.Scan0.ToPointer();//direkt bitmap memoriaba rajzolunk, gyors for (int i = 0; i < len; ++i) { cim[i] = buf[i]; } } bm.UnlockBits(bmData); return(bm); } }
public void SimulateModel(CarModelState state, CarModelInput input, double timeStep, out CarModelState output) { this.state = state; this.input = input; CarModelState state2 = state; double dAngle = (input.LeftSpeed - input.RightSpeed) * timeStep / CarModel.SHAFT_LENGTH; double lamda = 1; if (dAngle != 0) { lamda = 2 / dAngle * Math.Sin(dAngle / 2); } double vectLength = (input.RightSpeed + input.LeftSpeed) / 2 * timeStep * lamda; state2.Angle += dAngle; PointD p = new PointD((state.Position.X + vectLength * Math.Cos(state2.Angle - dAngle / 2)), (state.Position.Y + vectLength * Math.Sin(state2.Angle - dAngle / 2))); state2.Position = p; output = state2; }
private double[] obstacleFieldErrorGradient(CarModelState state, int time) { //C = sum((1/d(X) - 1/d(0))^2) //dC/dy_x =... //dC/dy_y =... double ksi = 0.0001; double errX = 0; double errY = 0; List <ObstacleState> obstacles = obstacleProvider.GetObstacleStates(time); foreach (ObstacleState obst in obstacles) { double x = ComMath.Normal(state.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE); double y = ComMath.Normal(state.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE); double x0 = ComMath.Normal(obst.pp.position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE); double y0 = ComMath.Normal(obst.pp.position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE); double r = ComMath.Normal(obst.radius + CarModel.SHAFT_LENGTH / 2, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE); double dist = Math.Sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0)) - r; if (dist <= 0.0001) { dist = 0.0001; } double err = (1 / (dist * dist * dist)); errX += err * (x - x0); errY += err * (y - y0); } if (obstacles.Count > 0) { errX /= obstacles.Count; errY /= obstacles.Count; } return(new double[] { ksi *errX, ksi *errY, 0, 0 }); }
private void init(CarModelState state) { this.state = state; int len = (int)(SHAFT_LENGTH / MM_PER_PIXEL / 2); graphicCarModelWheelLeft = new PointF[] { new PointF(-5, -len - 3), new PointF(5, -len - 3), new PointF(5, -len + 2), new PointF(-5, -len + 2) }; graphicCarModelWheelRight = new PointF[] { new PointF(-5, len - 2), new PointF(5, len - 2), new PointF(5, len + 3), new PointF(-5, len + 3) }; graphicCarModelBody = new PointF[] { new PointF(-1, -len + 2), new PointF(-len*2, -len + 2), new PointF(-len*2, len - 2), new PointF(-1, len - 2) }; }
public void SimulateModel(CarModelState state, CarModelInput input, out CarModelState output, out double[] NNOutput) { SimulateModel(state, input, CarModel.SIMULATION_TIME_STEP, out output, out NNOutput); }
//nem jo /* public void CalcErrorSensibility(double[] errors, out double[] sensitibility) { double dAngle = (input.RightSpeed - input.LeftSpeed) * CarModel.SIMULATION_TIME_STEP / CarModel.SHAFT_LENGTH; double lamda = 1; if (dAngle != 0) lamda = 4 / dAngle * Math.Sin(dAngle / 2); double vectLength = (input.RightSpeed + input.LeftSpeed) / 2 * CarModel.SIMULATION_TIME_STEP * lamda; //ez felfoghato egy forgataskent is: //vectLength * (state.Orientation.X *Math.Cos(-dAngle / 2) - state.Orientation.Y * Math.Sin(-dAngle / 2)) //vectLength * (state.Orientation.X *Math.Sin(-dAngle / 2) + state.Orientation.Y * Math.Cos(-dAngle / 2)) // PointD p = new PointD((state.Position.X + vectLength * Math.Cos(state.Angle - dAngle / 2)), (state.Position.Y + vectLength * Math.Sin(state.Angle - dAngle / 2))); //kimenetek bemenet szerinti derivaltjai double dAngle_rightSpeed = CarModel.SIMULATION_TIME_STEP / CarModel.SHAFT_LENGTH; double dAngle_leftSpeed = -CarModel.SIMULATION_TIME_STEP / CarModel.SHAFT_LENGTH; double lamda_rightSpeed = 0; double lamda_leftSpeed = 0; if (dAngle != 0) { lamda_rightSpeed = dAngle_rightSpeed * (-4 / Math.Pow(dAngle, 2) * Math.Sin(dAngle / 2) + 2 / dAngle * Math.Cos(dAngle / 2)); lamda_leftSpeed = dAngle_leftSpeed * (-4 / Math.Pow(dAngle, 2) * Math.Sin(dAngle / 2) + 2 / dAngle * Math.Cos(dAngle / 2)); } double vectLength_rightSpeed = lamda_rightSpeed * 1 / 2 * CarModel.SIMULATION_TIME_STEP * lamda; double vectLength_leftSpeed = lamda_leftSpeed * 1 / 2 * CarModel.SIMULATION_TIME_STEP * lamda; double outposX_inposX = 1; double outposX_inposY = 0; double outposX_inangX = vectLength * Math.Cos(- dAngle / 2); double outposX_inangY = - vectLength * Math.Sin(- dAngle / 2); double outposX_inrightSpeed = vectLength_rightSpeed * (state.Orientation.X *Math.Cos(-dAngle / 2) - state.Orientation.Y * Math.Sin(-dAngle / 2)) + dAngle_rightSpeed * vectLength * (-1 / 2) * (state.Orientation.X *(-Math.Sin(-dAngle / 2)) - state.Orientation.Y *Math.Cos(-dAngle / 2)); double outposX_inleftSpeed = vectLength_leftSpeed * (state.Orientation.X *Math.Cos(-dAngle / 2) - state.Orientation.Y * Math.Sin(-dAngle / 2)) + dAngle_leftSpeed * vectLength * (-1 / 2) * (state.Orientation.X *(-Math.Sin(-dAngle / 2)) + state.Orientation.Y *Math.Cos(-dAngle / 2)); double outposY_inposX = 0; double outposY_inposY = 1; double outposY_inangX = vectLength * Math.Sin(- dAngle / 2); double outposY_inangY = vectLength * Math.Cos(- dAngle / 2); double outposY_inrightSpeed = vectLength_rightSpeed * (state.Orientation.X *Math.Sin(-dAngle / 2) + state.Orientation.Y * Math.Cos(-dAngle / 2)) + dAngle_rightSpeed * vectLength * (-1 / 2) * (state.Orientation.X *Math.Cos(-dAngle / 2) - state.Orientation.Y *Math.Sin(-dAngle / 2)); double outposY_inleftSpeed = vectLength_leftSpeed * (state.Orientation.X * Math.Sin(-dAngle / 2) + state.Orientation.Y * Math.Cos(-dAngle / 2)) + dAngle_leftSpeed * vectLength * (-1 / 2) * (state.Orientation.X * Math.Cos(-dAngle / 2) - state.Orientation.Y * Math.Sin(-dAngle / 2)); double outangX_inposX = 0; double outangX_inposY = 0; double outangX_inangX = Math.Cos(dAngle); double outangX_inangY = -Math.Sin(dAngle); double outangX_inrightSpeed = dAngle_rightSpeed * (state.Orientation.X * (-Math.Sin(dAngle)) + state.Orientation.Y * (-Math.Cos(dAngle))); double outangX_inleftSpeed = dAngle_leftSpeed * (state.Orientation.X * (-Math.Sin(dAngle)) + state.Orientation.Y * (-Math.Cos(dAngle))); double outangY_inposX = 0; double outangY_inposY = 0; double outangY_inangX = Math.Sin(dAngle); double outangY_inangY = Math.Cos(dAngle); double outangY_inrightSpeed = dAngle_rightSpeed * (state.Orientation.X * Math.Cos(dAngle) - state.Orientation.Y * Math.Sin(dAngle)); double outangY_inleftSpeed = dAngle_leftSpeed * (state.Orientation.X * Math.Cos(dAngle) - state.Orientation.Y * Math.Sin(dAngle)); sensitibility = new double[6]; sensitibility[0] = (outposX_inposX * errors[0] + outposY_inposX * errors[1] + outangX_inposX * errors[2] + outangY_inposX * errors[3]); sensitibility[1] = (outposX_inposY * errors[0] + outposY_inposY * errors[1] + outangX_inposY * errors[2] + outangY_inposY * errors[3]); sensitibility[2] = (outposX_inangX * errors[0] + outposY_inangX * errors[1] + outangX_inangX * errors[2] + outangY_inangX * errors[3]); sensitibility[3] = (outposX_inangY * errors[0] + outposY_inangY * errors[1] + outangX_inangY * errors[2] + outangY_inangY * errors[3]); sensitibility[4] = (outposX_inrightSpeed * errors[0] + outposY_inrightSpeed * errors[1] + outangX_inrightSpeed * errors[2] + outangY_inrightSpeed * errors[3]); sensitibility[5] = (outposX_inleftSpeed * errors[0] + outposY_inleftSpeed * errors[1] + outangX_inleftSpeed * errors[2] + outangY_inleftSpeed * errors[3]); } */ public void CalcErrorSensibility(double[] errors, out double[] sensitibility) { CarModelState output1, output2, state1, state2, origiState = this.state; CarModelInput input1, input2, origiInput = this.input; double DIFF_C = 0.001; sensitibility = new double[7]; //****** //POS X //****** state1 = origiState; state1.Position = new PointD(ComMath.Normal(ComMath.Normal(state1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X), state1.Position.Y); this.SimulateModel(state1, origiInput, out output1); state2 = origiState; state2.Position = new PointD(ComMath.Normal(ComMath.Normal(state2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X), state2.Position.Y); this.SimulateModel(state2, origiInput, out output2); sensitibility[0] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] + (ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] + (ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] + (ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3]; //****** //POS Y //****** state1 = origiState; state1.Position = new PointD(state1.Position.X, ComMath.Normal(ComMath.Normal(state1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)); this.SimulateModel(state1, origiInput, out output1); state2 = origiState; state2.Position = new PointD(state2.Position.X, ComMath.Normal(ComMath.Normal(state2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)); this.SimulateModel(state2, origiInput, out output2); sensitibility[1] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] + (ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] + (ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] + (ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3]; //****** //ORIENTATION X //****** state1 = origiState; state1.Orientation = new PointD(ComMath.Normal(ComMath.Normal(state1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY), state1.Orientation.Y); this.SimulateModel(state1, origiInput, out output1); state2 = origiState; state2.Orientation = new PointD(ComMath.Normal(ComMath.Normal(state2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY), state2.Orientation.Y); this.SimulateModel(state2, origiInput, out output2); sensitibility[2] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] + (ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] + (ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] + (ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3]; //****** //ORIENTATION Y //****** state1 = origiState; state1.Orientation = new PointD(state1.Orientation.X, ComMath.Normal(ComMath.Normal(state1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY)); this.SimulateModel(state1, origiInput, out output1); state2 = origiState; state2.Orientation = new PointD(state2.Orientation.X, ComMath.Normal(ComMath.Normal(state2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY)); this.SimulateModel(state2, origiInput, out output2); sensitibility[3] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] + (ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] + (ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] + (ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3]; //****** //LEFT SPEED //****** input1 = origiInput; input1.LeftSpeed = ComMath.Normal(ComMath.Normal(input1.LeftSpeed, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED); this.SimulateModel(origiState, input1, out output1); input2 = origiInput; input2.LeftSpeed = ComMath.Normal(ComMath.Normal(input2.LeftSpeed, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED); this.SimulateModel(origiState, input2, out output2); sensitibility[4] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] + (ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] + (ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] + (ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3]; //****** //RIGHT SPEED //****** input1 = origiInput; input1.RightSpeed = ComMath.Normal(ComMath.Normal(input1.RightSpeed, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED); this.SimulateModel(origiState, input1, out output1); input2 = origiInput; input2.RightSpeed = ComMath.Normal(ComMath.Normal(input2.RightSpeed, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED); this.SimulateModel(origiState, input2, out output2); sensitibility[5] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] + (ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] + (ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] + (ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3]; //****** //WHEEL ANGLE //****** input1 = origiInput; input1.Angle = ComMath.Normal(ComMath.Normal(input1.Angle, CarModelInput.MIN_ANGLE, CarModelInput.MAX_ANGLE, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelInput.MIN_ANGLE, CarModelInput.MAX_ANGLE); this.SimulateModel(origiState, input1, out output1); input2 = origiInput; input2.Angle = ComMath.Normal(ComMath.Normal(input2.Angle, CarModelInput.MIN_ANGLE, CarModelInput.MAX_ANGLE, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelInput.MIN_ANGLE, CarModelInput.MAX_ANGLE); this.SimulateModel(origiState, input2, out output2); sensitibility[6] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] + (ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] + (ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] + (ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3]; this.state = origiState; this.input = origiInput; }
public void SimulateModel(CarModelState state, CarModelInput input, double timeStep, out CarModelState output) { double[] NNout; SimulateModel(state, input, timeStep, out output, out NNout); }
private void timer1_Tick(object sender, EventArgs e) { if (carRunning) { if (timerDiv == 0) { ICarPositionProvider carPos; IFinishPositionProvider finishPos; if (simulation) { itemManager.TakeSample(); carPos = itemManager; finishPos = itemManager; } else { cameraCarPosition.TakeSample(); carPos = cameraCarPosition; finishPos = cameraCarPosition; } //leallitas ha beert a celba double errx = carPos.GetCarState().Position.X - finishPos.GetFinishState(0).Position.X; double erry = carPos.GetCarState().Position.Y - finishPos.GetFinishState(0).Position.Y; double errox = carPos.GetCarState().Orientation.X - finishPos.GetFinishState(0).Orientation.X; double erroy = carPos.GetCarState().Orientation.Y - finishPos.GetFinishState(0).Orientation.Y; if ((errx * errx + erry * erry < CarModel.SHAFT_LENGTH * CarModel.SHAFT_LENGTH) && (errox * errox + erroy * erroy < 0.2)) { buttonStopSim_Click(this, null); } else { carModelGraphicControl1.SetReceiveCommand(); GridCarModelInput oi; GridCarModelState os; neuralController.SimulateOneStep(GridCarModelState.FromCarModelState(carPos.GetCarState()), out oi, out os); outState = GridCarModelState.ToCarModelState(os); outInput = new CarModelInput(oi.Angle); //outInput = new CarModelInput(20, 100); if (checkBoxSerial.Checked) { byte leftspd = (byte)Convert.ToSByte(ComMath.Normal(outInput.LeftSpeed, -180, 180, -128, 127)); byte rightspd = (byte)Convert.ToSByte(ComMath.Normal(outInput.RightSpeed, -180, 180, -128, 127)); //-125, 124 if (checkBoxCarEnable.Checked) { serialComm.Motor_I2C_Forward(1, leftspd, rightspd); } //Thread.Sleep(200); } } } timerDiv = (timerDiv + 1) % (long)(CarModel.SIMULATION_TIME_STEP * 1000.0 / timer1.Interval); if (simulation) { //itemManager.Simulate(new MathModelSimulator(), outInput, timer1.Interval / 1000.0); itemManager.SimualteGrid(new GridMathModelSimulator(), new GridCarModelInput(outInput.LeftSpeed, outInput.RightSpeed), timer1.Interval / 1000.0); } else { cameraCarPosition.Simulate(new MathModelSimulator(), outInput, timer1.Interval / 1000.0); } } carModelGraphicControl1.Invalidate(); }
private void RunMarkerFinder() { if (mf != null) { Bitmap frame = camera.GetBitmap(); List <Shape> shapes = null; shapes = mf.ProcessFrame(frame); bool carFound = false; bool finishFound = false; // finish currentObstacleStates = new List <ObstacleState>(); foreach (Shape s in shapes) { if ((s.index == 0) && (s.scale > 0.17) && (s.scale < 0.22)) { currentCarModelState = new CarModelState(new PointD(ComMath.Normal(s.pos.X, 0, frame.Width, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X), ComMath.Normal(s.pos.Y, 0, frame.Height, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)), -Math.PI / 2 - s.rot); carFound = true; } else if ((s.index == 1) && (s.scale > 0.16) && (s.scale < 0.20)) { currentFinishState = new FinishState(new PointD(ComMath.Normal(s.pos.X, 0, frame.Width, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X), ComMath.Normal(s.pos.Y, 0, frame.Height, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)), -Math.PI / 2 - s.rot); finishPredictor.AddPoint(currentFinishState.Position, currentFinishState.Orientation); finishFound = true; } else { currentObstacleStates.Add(new ObstacleState(new PointD(ComMath.Normal(s.pos.X, 0, frame.Width, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X), ComMath.Normal(s.pos.Y, 0, frame.Height, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)), s.scale * 500)); //ide kell egy robusztus alg //ha zaj jon, akkor ne vegye be } } lock (obstacles) { List <ObstacleState> curObState = new List <ObstacleState>(currentObstacleStates); foreach (ObstacleModel ops in obstacles) { List <PointD> list = ops.state.pp.PredictNextPositions(1); ObstacleState osw = null; PointD pd; if (list != null) { pd = list[0]; } else { pd = ops.state.pp.position; } double mindist = double.MaxValue; foreach (ObstacleState os in curObState) { double dist = (os.pp.position.X - pd.X) * (os.pp.position.X - pd.X) + (os.pp.position.Y - pd.Y) * (os.pp.position.Y - pd.Y); if (dist < mindist) { mindist = dist; osw = os; } } if (osw != null) { lock (ops) { ops.state.pp.AddNewPosition(osw.pp.position); } curObState.Remove(osw); } else { //a predikcioval leptetjuk tovabb lock (ops) { ops.state.pp.AddNewPosition(pd); } } } foreach (ObstacleState os in curObState) { lock (obstacles) { ObstacleModel om = new ObstacleModel(os.pp.position, os.radius); om.SetSelectedState(1, 0); if (obstacles.Count < 1) { obstacles.Add(om); } } } } } }
//public CarModelState GetState(int index) //{ // double[] ret = GetPosition(index); // CarModelState cs = new CarModelState(); // cs.Angle = -ret[2]; // //cs.Position = new PointD(ComMath.Normal(ret[0], 0, im.Width, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X), ComMath.Normal(im.Height - ret[1], 0, im.Height, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)); // return cs; //} public void TakeSample() { RunMarkerFinder(); sampledCarState = currentCarModelState; }
private double[] obstacleFieldErrorGradient(CarModelState state, int time) { //C = sum((1/d(X) - 1/d(0))^2) //dC/dy_x =... //dC/dy_y =... double ksi = 0.0001; double errX = 0; double errY = 0; List<ObstacleState> obstacles = obstacleProvider.GetObstacleStates(time); foreach (ObstacleState obst in obstacles) { double x = ComMath.Normal(state.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE); double y = ComMath.Normal(state.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE); double x0 = ComMath.Normal(obst.pp.position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE); double y0 = ComMath.Normal(obst.pp.position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE); double r = ComMath.Normal(obst.radius + CarModel.SHAFT_LENGTH/2, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE); double dist = Math.Sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0)) - r; if (dist <= 0.0001) dist = 0.0001; double err = (1 / (dist * dist * dist)); errX += err * (x - x0); errY += err * (y - y0); } if (obstacles.Count > 0) { errX /= obstacles.Count; errY /= obstacles.Count; } return new double[] { ksi * errX, ksi * errY, 0, 0 }; }
//matematikai modell public void SimulateModel(CarModelState state, CarModelInput input, out CarModelState output) { SimulateModel(state, input, CarModel.SIMULATION_TIME_STEP, out output); }
private void timer1_Tick(object sender, EventArgs e) { if (carRunning) { if (timerDiv == 0) { ICarPositionProvider carPos; IFinishPositionProvider finishPos; if (simulation) { itemManager.TakeSample(); carPos = itemManager; finishPos = itemManager; } else { cameraCarPosition.TakeSample(); carPos = cameraCarPosition; finishPos = cameraCarPosition; } //leallitas ha beert a celba double errx = carPos.GetCarState().Position.X - finishPos.GetFinishState(0).Position.X; double erry = carPos.GetCarState().Position.Y - finishPos.GetFinishState(0).Position.Y; double errox = carPos.GetCarState().Orientation.X - finishPos.GetFinishState(0).Orientation.X; double erroy = carPos.GetCarState().Orientation.Y - finishPos.GetFinishState(0).Orientation.Y; if ((errx * errx + erry * erry < CarModel.SHAFT_LENGTH * CarModel.SHAFT_LENGTH) && (errox * errox + erroy * erroy < 0.2)) { buttonStopSim_Click(this, null); } else { carModelGraphicControl1.SetReceiveCommand(); GridCarModelInput oi; GridCarModelState os; neuralController.SimulateOneStep(GridCarModelState.FromCarModelState(carPos.GetCarState()), out oi, out os); outState = GridCarModelState.ToCarModelState(os); outInput = new CarModelInput(oi.Angle); //outInput = new CarModelInput(20, 100); if (checkBoxSerial.Checked) { byte leftspd = (byte)Convert.ToSByte(ComMath.Normal(outInput.LeftSpeed, -180, 180, -128, 127)); byte rightspd = (byte)Convert.ToSByte(ComMath.Normal(outInput.RightSpeed, -180, 180, -128, 127)); //-125, 124 if (checkBoxCarEnable.Checked) serialComm.Motor_I2C_Forward(1, leftspd, rightspd); //Thread.Sleep(200); } } } timerDiv = (timerDiv + 1) % (long)(CarModel.SIMULATION_TIME_STEP * 1000.0 / timer1.Interval); if (simulation) { //itemManager.Simulate(new MathModelSimulator(), outInput, timer1.Interval / 1000.0); itemManager.SimualteGrid(new GridMathModelSimulator(), new GridCarModelInput(outInput.LeftSpeed, outInput.RightSpeed), timer1.Interval / 1000.0); } else { cameraCarPosition.Simulate(new MathModelSimulator(), outInput, timer1.Interval / 1000.0); } } carModelGraphicControl1.Invalidate(); }
public void SimulateOneStep(CarModelState state, out CarModelInput outInput, out CarModelState outState) { NeuralController.SimulateOneStep(this.controller, this.model, state, out outInput, out outState); }
public CarModel(CarModelState state, Color c) { mainColor = c; init(state); }
private double TrainOneEpoch(double mu, out double SumSimCount, out List <CarModelState> innerStates) { int maxSimCount = 100; double sumSimCount = 0; double error = 0; innerStates = new List <CarModelState>(); List <double> deltaws = new List <double>(); MLPDll[] controllers = new MLPDll[maxSimCount]; IModelSimulator[] models = new IModelSimulator[maxSimCount]; CarModelState state = carStateProvider.GetCarState(); CarModelInput input = new CarModelInput(); //kimenet kiszamitasa int simCount = 0; List <double[]> singleErrors = new List <double[]>(); List <double[]> regularizationErrors = new List <double[]>(); CarModelState laststate; bool earlyStop; do { controllers[simCount] = new MLPDll(controller); //lemasoljuk models[simCount] = model.Clone(); //a modellt is laststate = state; NeuralController.SimulateOneStep(controllers[simCount], models[simCount], state, out input, out state);//vegigszimulaljuk a simCount darab controlleren es modellen innerStates.Add(state); //kozbulso hibak kiszamitasa, itt csak az akadalyoktol valo tavolsag "hibajat" vesszuk figyelembe, irany nem szamit -> hibaja 0 regularizationErrors.Add(obstacleFieldErrorGradient(state, simCount)); //minden pont celtol vett tavolsaga double[] desiredOutput = (double[])finishStateProvider.GetFinishState(simCount); singleErrors.Add(new double[] { 1 * ComMath.Normal(desiredOutput[0] - state.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE), 1 * ComMath.Normal(desiredOutput[1] - state.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE), 0.1 * ComMath.Normal(desiredOutput[2] - state.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE), 0.1 * ComMath.Normal(desiredOutput[3] - state.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE) } ); ++simCount; earlyStop = false; if (simCount > 3) { double[] err1 = singleErrors[simCount - 1]; double[] err2 = singleErrors[simCount - 2]; double[] err3 = singleErrors[simCount - 3]; double error1, error2, error3; error1 = error2 = error3 = 0; for (int i = 0; i < err1.Length; i++) { error1 += err1[i] * err1[i]; error2 += err2[i] * err2[i]; error3 += err3[i] * err3[i]; } earlyStop = ((error1 > error2) && (error3 > error2)); if (earlyStop) { //utolso elemet toroljuk singleErrors.RemoveAt(singleErrors.Count - 1); regularizationErrors.RemoveAt(regularizationErrors.Count - 1); innerStates.RemoveAt(innerStates.Count - 1); --simCount; } } }while ((simCount < maxSimCount) && !earlyStop); double[] errors = singleErrors[singleErrors.Count - 1]; sumSimCount += simCount; //hibavisszaterjesztes for (int i = simCount - 1; i >= 0; --i) { double[] sensitibility; models[i].CalcErrorSensibility(errors, out sensitibility); double[] inputSensitibility; if (INPUT_TYPE == inputType.wheelAngle) { inputSensitibility = new double[1]; inputSensitibility[0] = sensitibility[6]; } else if (INPUT_TYPE == inputType.wheelSpeed) { inputSensitibility = new double[2]; inputSensitibility[0] = sensitibility[4]; inputSensitibility[1] = sensitibility[5]; } double[] sensitibility2; controllers[i].SetOutputError(inputSensitibility); controllers[i].Backpropagate(); controllers[i].CalculateDeltaWeights(); sensitibility2 = controllers[i].SensitibilityD(); errors[0] = (sensitibility[0] + sensitibility2[0]); errors[1] = (sensitibility[1] + sensitibility2[1]); errors[2] = (sensitibility[2] + sensitibility2[2]); errors[3] = (sensitibility[3] + sensitibility2[3]); //regularizaciobol szarmazo hiba hozzaadasa errors[0] += regularizationErrors[i][0]; errors[1] += regularizationErrors[i][1]; } controller.ClearDeltaWeights(); //sulymodositasok osszegzese for (int i2 = 0; i2 < simCount; ++i2) { controller.AddDeltaWeights(controllers[i2]); } float maxdw = controller.MaxDeltaWeight(); //if (maxdw < 50) maxdw = 50; controller.ChangeWeights(mu / maxdw); ////sulymodositasok osszegzese //for (int i2 = 0; i2 < simCount; ++i2) //simCount //{ // int count = 0; // for (int i = 1; i < controllers[i2]; ++i) // { // foreach (INeuron n in controllers[i2].mlp[i]) // { // foreach (NeuronInput ni in ((Neuron)n).inputs) // { // if (deltaws.Count <= count) deltaws.Add(ni.deltaw); // else deltaws[count] += ni.deltaw; // ++count; // } // } // } //} ////legnagyobb sulymodositas ertekenek meghatarozasa, majd ezzel normalas //double maxdw = 1; //foreach (double dw in deltaws) //{ // if (Math.Abs(dw) > maxdw) maxdw = Math.Abs(dw); //} //if (maxdw < 50) maxdw = 50; ////sulymodositasok ervenyre juttatasa a controllerben //int count2 = 0; //for (int i = 1; i < controller.mlp.Count; ++i) //{ // foreach (INeuron n in controller.mlp[i]) // { // foreach (NeuronInput ni in ((Neuron)n).inputs) // { // ni.w += mu * deltaws[count2] / maxdw; // ++count2; // } // } //} SumSimCount = sumSimCount; return(error); }
public CarModel(CarModelState state) { mainColor = Color.Gray; init(state); }
//nem jo /* * public void CalcErrorSensibility(double[] errors, out double[] sensitibility) * { * double dAngle = (input.RightSpeed - input.LeftSpeed) * CarModel.SIMULATION_TIME_STEP / CarModel.SHAFT_LENGTH; * double lamda = 1; * if (dAngle != 0) lamda = 4 / dAngle * Math.Sin(dAngle / 2); * double vectLength = (input.RightSpeed + input.LeftSpeed) / 2 * CarModel.SIMULATION_TIME_STEP * lamda; * * * //ez felfoghato egy forgataskent is: * //vectLength * (state.Orientation.X *Math.Cos(-dAngle / 2) - state.Orientation.Y * Math.Sin(-dAngle / 2)) * //vectLength * (state.Orientation.X *Math.Sin(-dAngle / 2) + state.Orientation.Y * Math.Cos(-dAngle / 2)) * // * PointD p = new PointD((state.Position.X + vectLength * Math.Cos(state.Angle - dAngle / 2)), * (state.Position.Y + vectLength * Math.Sin(state.Angle - dAngle / 2))); * * //kimenetek bemenet szerinti derivaltjai * double dAngle_rightSpeed = CarModel.SIMULATION_TIME_STEP / CarModel.SHAFT_LENGTH; * double dAngle_leftSpeed = -CarModel.SIMULATION_TIME_STEP / CarModel.SHAFT_LENGTH; * double lamda_rightSpeed = 0; * double lamda_leftSpeed = 0; * if (dAngle != 0) * { * lamda_rightSpeed = dAngle_rightSpeed * (-4 / Math.Pow(dAngle, 2) * Math.Sin(dAngle / 2) + 2 / dAngle * Math.Cos(dAngle / 2)); * lamda_leftSpeed = dAngle_leftSpeed * (-4 / Math.Pow(dAngle, 2) * Math.Sin(dAngle / 2) + 2 / dAngle * Math.Cos(dAngle / 2)); * } * double vectLength_rightSpeed = lamda_rightSpeed * 1 / 2 * CarModel.SIMULATION_TIME_STEP * lamda; * double vectLength_leftSpeed = lamda_leftSpeed * 1 / 2 * CarModel.SIMULATION_TIME_STEP * lamda; * * * double outposX_inposX = 1; * double outposX_inposY = 0; * double outposX_inangX = vectLength * Math.Cos(- dAngle / 2); * double outposX_inangY = - vectLength * Math.Sin(- dAngle / 2); * double outposX_inrightSpeed = vectLength_rightSpeed * (state.Orientation.X *Math.Cos(-dAngle / 2) - state.Orientation.Y * Math.Sin(-dAngle / 2)) + dAngle_rightSpeed * vectLength * (-1 / 2) * (state.Orientation.X *(-Math.Sin(-dAngle / 2)) - state.Orientation.Y *Math.Cos(-dAngle / 2)); * double outposX_inleftSpeed = vectLength_leftSpeed * (state.Orientation.X *Math.Cos(-dAngle / 2) - state.Orientation.Y * Math.Sin(-dAngle / 2)) + dAngle_leftSpeed * vectLength * (-1 / 2) * (state.Orientation.X *(-Math.Sin(-dAngle / 2)) + state.Orientation.Y *Math.Cos(-dAngle / 2)); * * double outposY_inposX = 0; * double outposY_inposY = 1; * double outposY_inangX = vectLength * Math.Sin(- dAngle / 2); * double outposY_inangY = vectLength * Math.Cos(- dAngle / 2); * double outposY_inrightSpeed = vectLength_rightSpeed * (state.Orientation.X *Math.Sin(-dAngle / 2) + state.Orientation.Y * Math.Cos(-dAngle / 2)) + dAngle_rightSpeed * vectLength * (-1 / 2) * (state.Orientation.X *Math.Cos(-dAngle / 2) - state.Orientation.Y *Math.Sin(-dAngle / 2)); * double outposY_inleftSpeed = vectLength_leftSpeed * (state.Orientation.X * Math.Sin(-dAngle / 2) + state.Orientation.Y * Math.Cos(-dAngle / 2)) + dAngle_leftSpeed * vectLength * (-1 / 2) * (state.Orientation.X * Math.Cos(-dAngle / 2) - state.Orientation.Y * Math.Sin(-dAngle / 2)); * * * double outangX_inposX = 0; * double outangX_inposY = 0; * double outangX_inangX = Math.Cos(dAngle); * double outangX_inangY = -Math.Sin(dAngle); * double outangX_inrightSpeed = dAngle_rightSpeed * (state.Orientation.X * (-Math.Sin(dAngle)) + state.Orientation.Y * (-Math.Cos(dAngle))); * double outangX_inleftSpeed = dAngle_leftSpeed * (state.Orientation.X * (-Math.Sin(dAngle)) + state.Orientation.Y * (-Math.Cos(dAngle))); * * double outangY_inposX = 0; * double outangY_inposY = 0; * double outangY_inangX = Math.Sin(dAngle); * double outangY_inangY = Math.Cos(dAngle); * double outangY_inrightSpeed = dAngle_rightSpeed * (state.Orientation.X * Math.Cos(dAngle) - state.Orientation.Y * Math.Sin(dAngle)); * double outangY_inleftSpeed = dAngle_leftSpeed * (state.Orientation.X * Math.Cos(dAngle) - state.Orientation.Y * Math.Sin(dAngle)); * * sensitibility = new double[6]; * sensitibility[0] = (outposX_inposX * errors[0] + outposY_inposX * errors[1] + outangX_inposX * errors[2] + outangY_inposX * errors[3]); * sensitibility[1] = (outposX_inposY * errors[0] + outposY_inposY * errors[1] + outangX_inposY * errors[2] + outangY_inposY * errors[3]); * sensitibility[2] = (outposX_inangX * errors[0] + outposY_inangX * errors[1] + outangX_inangX * errors[2] + outangY_inangX * errors[3]); * sensitibility[3] = (outposX_inangY * errors[0] + outposY_inangY * errors[1] + outangX_inangY * errors[2] + outangY_inangY * errors[3]); * sensitibility[4] = (outposX_inrightSpeed * errors[0] + outposY_inrightSpeed * errors[1] + outangX_inrightSpeed * errors[2] + outangY_inrightSpeed * errors[3]); * sensitibility[5] = (outposX_inleftSpeed * errors[0] + outposY_inleftSpeed * errors[1] + outangX_inleftSpeed * errors[2] + outangY_inleftSpeed * errors[3]); * } */ public void CalcErrorSensibility(double[] errors, out double[] sensitibility) { CarModelState output1, output2, state1, state2, origiState = this.state; CarModelInput input1, input2, origiInput = this.input; double DIFF_C = 0.001; sensitibility = new double[7]; //****** //POS X //****** state1 = origiState; state1.Position = new PointD(ComMath.Normal(ComMath.Normal(state1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X), state1.Position.Y); this.SimulateModel(state1, origiInput, out output1); state2 = origiState; state2.Position = new PointD(ComMath.Normal(ComMath.Normal(state2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X), state2.Position.Y); this.SimulateModel(state2, origiInput, out output2); sensitibility[0] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] + (ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] + (ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] + (ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3]; //****** //POS Y //****** state1 = origiState; state1.Position = new PointD(state1.Position.X, ComMath.Normal(ComMath.Normal(state1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)); this.SimulateModel(state1, origiInput, out output1); state2 = origiState; state2.Position = new PointD(state2.Position.X, ComMath.Normal(ComMath.Normal(state2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)); this.SimulateModel(state2, origiInput, out output2); sensitibility[1] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] + (ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] + (ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] + (ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3]; //****** //ORIENTATION X //****** state1 = origiState; state1.Orientation = new PointD(ComMath.Normal(ComMath.Normal(state1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY), state1.Orientation.Y); this.SimulateModel(state1, origiInput, out output1); state2 = origiState; state2.Orientation = new PointD(ComMath.Normal(ComMath.Normal(state2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY), state2.Orientation.Y); this.SimulateModel(state2, origiInput, out output2); sensitibility[2] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] + (ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] + (ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] + (ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3]; //****** //ORIENTATION Y //****** state1 = origiState; state1.Orientation = new PointD(state1.Orientation.X, ComMath.Normal(ComMath.Normal(state1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY)); this.SimulateModel(state1, origiInput, out output1); state2 = origiState; state2.Orientation = new PointD(state2.Orientation.X, ComMath.Normal(ComMath.Normal(state2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY)); this.SimulateModel(state2, origiInput, out output2); sensitibility[3] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] + (ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] + (ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] + (ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3]; //****** //LEFT SPEED //****** input1 = origiInput; input1.LeftSpeed = ComMath.Normal(ComMath.Normal(input1.LeftSpeed, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED); this.SimulateModel(origiState, input1, out output1); input2 = origiInput; input2.LeftSpeed = ComMath.Normal(ComMath.Normal(input2.LeftSpeed, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED); this.SimulateModel(origiState, input2, out output2); sensitibility[4] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] + (ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] + (ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] + (ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3]; //****** //RIGHT SPEED //****** input1 = origiInput; input1.RightSpeed = ComMath.Normal(ComMath.Normal(input1.RightSpeed, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED); this.SimulateModel(origiState, input1, out output1); input2 = origiInput; input2.RightSpeed = ComMath.Normal(ComMath.Normal(input2.RightSpeed, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED); this.SimulateModel(origiState, input2, out output2); sensitibility[5] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] + (ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] + (ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] + (ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3]; //****** //WHEEL ANGLE //****** input1 = origiInput; input1.Angle = ComMath.Normal(ComMath.Normal(input1.Angle, CarModelInput.MIN_ANGLE, CarModelInput.MAX_ANGLE, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelInput.MIN_ANGLE, CarModelInput.MAX_ANGLE); this.SimulateModel(origiState, input1, out output1); input2 = origiInput; input2.Angle = ComMath.Normal(ComMath.Normal(input2.Angle, CarModelInput.MIN_ANGLE, CarModelInput.MAX_ANGLE, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) + DIFF_C / 2, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE, CarModelInput.MIN_ANGLE, CarModelInput.MAX_ANGLE); this.SimulateModel(origiState, input2, out output2); sensitibility[6] = (ComMath.Normal(output2.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[0] + (ComMath.Normal(output2.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[1] + (ComMath.Normal(output2.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[2] + (ComMath.Normal(output2.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE) - ComMath.Normal(output1.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, NeuralController.MIN_NEURON_VALUE, NeuralController.MAX_NEURON_VALUE)) / DIFF_C * errors[3]; this.state = origiState; this.input = origiInput; }
public static void SimulateOneStep(MLPDll controller, IModelSimulator model, CarModelState state, out CarModelInput outInput, out CarModelState outState) { double[] inputs = new double[4]; inputs[0] = ComMath.Normal(state.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, POSITION_SCALE * MIN_NEURON_VALUE, POSITION_SCALE * MAX_NEURON_VALUE); inputs[1] = ComMath.Normal(state.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, POSITION_SCALE * MIN_NEURON_VALUE, POSITION_SCALE * MAX_NEURON_VALUE); inputs[2] = ComMath.Normal(state.Orientation.X, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE); inputs[3] = ComMath.Normal(state.Orientation.Y, CarModelState.MIN_OR_XY, CarModelState.MAX_OR_XY, MIN_NEURON_VALUE, MAX_NEURON_VALUE); double[] controllerOutputs = controller.Output(inputs); if (INPUT_TYPE == inputType.wheelAngle) { outInput = new CarModelInput(); outInput.Angle = ComMath.Normal(controllerOutputs[0], MIN_NEURON_VALUE, MAX_NEURON_VALUE, CarModelInput.MIN_ANGLE, CarModelInput.MAX_ANGLE); } else if (INPUT_TYPE == inputType.wheelSpeed) { outInput = new CarModelInput(ComMath.Normal(controllerOutputs[0], MIN_NEURON_VALUE, MAX_NEURON_VALUE, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED), ComMath.Normal(controllerOutputs[1], MIN_NEURON_VALUE, MAX_NEURON_VALUE, CarModelInput.MIN_SPEED, CarModelInput.MAX_SPEED)); //******** //hatrafele tilos mennie if (outInput.LeftSpeed < 0) outInput.LeftSpeed = 0; if (outInput.RightSpeed < 0) outInput.RightSpeed = 0; //******** } model.SimulateModel(state, outInput, out outState); }
public void TakeSample() { sampledCarState = model.state; }
private double obstacleFieldError(CarModelState state) { double err = 0; List<ObstacleState> obstacles = obstacleProvider.GetObstacleStates(1); foreach (ObstacleState obst in obstacles) { double x = ComMath.Normal(state.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE); double y = ComMath.Normal(state.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE); double x0 = ComMath.Normal(obst.pp.position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE); double y0 = ComMath.Normal(obst.pp.position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE); double dist = Math.Sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0)); err += Math.Pow(1 / dist - 1 / obst.radius, 2); } return err; }
//public 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; } }
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); } } } } }