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