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 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 double Train(double[] inputValues)
{
if (inputValues.Length >= dimension * (inputLength + 1))
{
double error = 0;
for (int i = inputLength; i < inputValues.Length / dimension; ++i)
{
double[] input = new double[dimension * inputLength];
int i3 = 0;
for (int i2 = (i - inputLength) * dimension; i2 < i * dimension; ++i2)
{
input[i3] = inputValues[i2];
i3++;
}
double[] outp = mlp.Output(input);
double[] err = new double[dimension];
for (int i2 = 0; i2 < dimension; ++i2)
{
err[i2] = inputValues[i * dimension + i2] - outp[i2];
error += err[i2] * err[i2];
}
mlp.Train(mu, err);
trainCount++;
}
//if (error > 1.2 * preverror)
//{
// if (trainCount > 20000) mu *= 0.2;
//}
//else if (error < 0.8 * preverror)
//{
// mu *= 1.2;
//}
preverror = error / inputValues.Length * dimension;
return(preverror);
}
else
{
throw new Exception("Not enough data!");
}
}
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);
}
