robot.cs

kalman filter simulator

using System;
using System.Collections.Generic;
using System.Text;

namespace Kalman
{
    class Robot
    {
        public static double TrueX = 0; //Hidden
        public static double TrueY = 0; //Hidden

        public static double mX = 0; //Velocity
        public static double mY = 0; //Velocity

        public static double mXA = 0; //Acceleration
        public static double mYA = 0; //Acceleration

        public static void Step(double Noise, double deltaT)
        {
            mX += mXA;
            mY += mYA;
            mXA = 0;
            mYA = 0;

            TrueX += deltaT * mX + RandomProvider.NextNormal() * Noise;
            TrueY += deltaT * mY + RandomProvider.NextNormal() * Noise;
        }
    }
}