public static void OdometerPredict(KalmanFilter kalman, double dx, double dtheta) { var tmp = new double[] { dx, dtheta }; Vector<double> control = new DenseVector(tmp); var noise = new double[,] { {0.1, 0, 0}, {0, 0.1, 0}, {0, 0, 0.2}}; kalman.Predict(control, StateTransitionFunc, StateTransitionJacobianFunc, DenseMatrix.OfArray(noise)); }
public void MarkerUpdate(KalmanFilter kalman, Vector <double> measurement) { var noise = DenseMatrix.OfArray(new [, ] { { 0.01, 0 }, { 0, 0.01 }, }); kalman.Update(measurement, Measurement, MeasurementJacobian, noise); }