private double h; // Step size
public FunctionIntegrator(IntegratorFunction function, Range <double> interval, int NSteps)
{
func = new IntegratorFunction(function);
range = new Range <double>(interval.low, interval.high);
N = NSteps;
h = range.spread / (double)N;
}
public static Vector <double> RungeKutta4(IntegratorFunction f,
Vector <double> y0, double t0, double h)
{
Vector <double> k1 = f(t0, y0);
Vector <double> k2 = f(t0 + h / 2, y0 + h / 2 * k1);
Vector <double> k3 = f(t0 + h / 2, y0 + h / 2 * k2);
Vector <double> k4 = f(t0 + h, y0 + h * k3);
return(y0 + h / 6 * (k1 + 2 * k2 + 2 * k3 + k4));
}
private void IntegrateState()
{
IntegratorFunction f = delegate(double t, Vector <double> y) {
Vector <double> q = new DenseVector(
new double[] { y[0], y[1], y[2], y[3] });
_qd = A * q;
return(new DenseVector(new double[] {
_qd[0],
0, // steer accel is zero
_qd[2],
0, // steer rate is zero as we use the handlebar dynamics
(_v * y[3] +
(_param.trail * y[1] *
Math.Cos(_param.steerAxisTilt) / _param.wheelbase)),
_v * Math.Cos(y[4]),
_v * Math.Sin(y[4]),
_lastSensor.wheelRate
}));
};
long dt_ms = _lastSensor.timestamp_ms - _timestamp_ms;
if (dt_ms > 2 * _sim_period_ms)
{
// Data not received for a while. Skip integration step.
dt_ms = 0;
}
else if (dt_ms > 0)
{
// assume the data was on time and use the nominal time step
dt_ms = _sim_period_ms;
}
// Use the most recent measured handlebar steer angle and rate
_state.steerRate = _lastSensor.steerRate;
_state.steer = _lastSensor.steerAngle;
// Since y[1], y[3] are set to zero, steer states do not change
_state.vector = Integrator.RungeKutta4(f, _state.vector, 0,
Convert.ToDouble(dt_ms) / 1000);
_timestamp_ms = _lastSensor.timestamp_ms;
_elapsed_ms += dt_ms;
}
