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; }