demo.cs

kalman filter simulator

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Text;
using System.Windows.Forms;
using Mapack;

namespace Kalman
{
    public partial class Demo : Form
    {
        KalmanFilter filterX = new KalmanFilter(2, 1, 1);
        KalmanFilter filterY = new KalmanFilter(2, 1, 1);
        double x_input = 0, y_input = 0;
        double deltaT = .1;

        public Demo()
        {
            InitializeComponent();
            this.Capture = true;


            filterX.StateUpdateMatrix = new Matrix(new double[][] { new double[] { 1, deltaT }, new double[] { 0, 1 } });
            filterX.OutputMatrix = new Matrix(new double[][] { new double[] { 1, 0 } });
            filterX.ControlMatrix = new Matrix(new double[][] { new double[] { 0 }, new double[] { 1 } });
            filterX.SetInitialConfidence(1);

            filterY.StateUpdateMatrix = new Matrix(new double[][] { new double[] { 1, deltaT }, new double[] { 0, 1 } });
            filterY.OutputMatrix = new Matrix(new double[][] { new double[] { 1, 0 } });
            filterY.ControlMatrix = new Matrix(new double[][] { new double[] { 0 }, new double[] { 1 } });
            filterY.SetInitialConfidence(1);

            Robot.TrueX = this.Width / 2;
            Robot.TrueY = this.Height / 2;

            filterX[0] = Robot.TrueX;
            filterY[0] = Robot.TrueY;

            this.Paint += new PaintEventHandler(Demo_Paint);
            this.KeyDown += new KeyEventHandler(Demo_KeyDown);
        }

        void Demo_KeyDown(object sender, KeyEventArgs e)
        {
            switch(e.KeyCode)
            {
                case Keys.Up:
                    Robot.mYA = -1;
                    e.Handled = true;
                    break;
                case Keys.Down:
                    Robot.mYA = 1;
                    e.Handled = true;
                    break;
                case Keys.Left:
                    Robot.mXA = -1;
                    e.Handled = true;
                    break;
                case Keys.Right:
                    Robot.mXA = 1;
                    e.Handled = true;
                    break;
            }
        }


        void Demo_Paint(object sender, PaintEventArgs e)
        {
            Graphics g = Graphics.FromHwnd(this.Handle);

            g.DrawEllipse(Pens.Green, (int)Robot.TrueX-12, (int)Robot.TrueY-12, 25, 25);
            g.DrawEllipse(Pens.Blue, (int)filterX[0]-7, (int)filterY[0]-7, 15, 15);
            g.DrawRectangle(Pens.Blue, (int)x_input-2, (int)y_input-2, 5, 5);
        }

        private void timer1_Tick(object sender, EventArgs e)
        {
            filterX.SetControl(0, Robot.mXA);
            filterY.SetControl(0, Robot.mYA);
            
            Robot.Step((double)ProcessNoise.Value, deltaT);

            x_input = Robot.TrueX + (RandomProvider.NextNormal() * (int)Noise.Value);
            y_input = Robot.TrueY + (RandomProvider.NextNormal() * (int)Noise.Value);

            filterX.SetMeasurementNoise(new double[] { (double)Noise.Value });
            filterX.SetProcessNoise(new double[] { (double)ProcessNoise.Value, (double)ProcessNoise.Value });
            filterX.SetOutput(0, x_input);
            filterX.Predict();
            filterX.Update();

            filterY.SetMeasurementNoise(new double[] { (double)Noise.Value });
            filterY.SetProcessNoise(new double[] { (double)ProcessNoise.Value, (double)ProcessNoise.Value });
            filterY.SetOutput(0, y_input);
            filterY.Predict();
            filterY.Update();

            this.Refresh();
        }
    }
}