form1.cs

在Visual 2008环境下

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

namespace kalman_example
{
    public partial class Form1 : Form
    {
        private int[] _customColors = null;
        private Color PicBackColor;
        private Color RealPointColor;
        private Color MeasurePointColor;
        private Color PredictPointColor;
        private Color IndexLineColor;
        private int timeInterval;

        private kalmanFilter kalman = null;
        private const int dynam_dims = 2;
        private const int measure_dims = 1;

        private BackgroundWorker backworker1 = null;
        private bool isbackworkerHasWork = false;
        private bool isStop = false;
        private bool isPause = false;

        public Form1()
        {
            InitializeComponent();

            this.Text = "卡尔曼预测示例";

            PicBackColor = PicBackColorBox.BackColor;
            RealPointColor = RealPointColorBox.BackColor;
            MeasurePointColor = MeasurePointColorBox.BackColor;
            PredictPointColor = PredictPointColorBox.BackColor;
            IndexLineColor = IndexLineColortextBox.BackColor;
            timeInterval = Convert.ToInt32(textBoxTimeInterval.Text);
        }

        private void buttonShow_Click(object sender, EventArgs e)
        {
            if (!isbackworkerHasWork)
            {
                isbackworkerHasWork = false;
                isStop = false;
                isPause = false;

                buttonPicBackColorSel.Enabled = false;
                buttonRealPointColorSel.Enabled = false;
                buttonMeasurePointColorSel.Enabled = false;
                buttonPredictPointColorSel.Enabled = false;
                buttonIndexLineColorSel.Enabled = false;
                textBoxTimeInterval.Enabled = false;

                backworker1 = new BackgroundWorker();
                backworker1.DoWork += new DoWorkEventHandler(backworker1_DoWork);
                backworker1.RunWorkerCompleted += new RunWorkerCompletedEventHandler(backworker1_RunWorkerCompleted);
                backworker1.RunWorkerAsync();

                buttonPause.Enabled = true;
                buttonShow.Text = "stop";
            }
            else
            {
                isStop = true;
                isbackworkerHasWork = false;
                buttonShow.Text = "show";
            }
        }

        private void backworker1_DoWork(object sender, DoWorkEventArgs e)
        {
            isbackworkerHasWork = true;

            Bitmap showBmp = null;
            Size bmSize = pictureBoxShow.Size;

            Random rand = new Random();

            // 设置状态转移矩阵、 观测矩阵、 过程噪声的协方差矩阵、 观测噪声的协方差矩阵、 
            //状态矩阵、 后验状态矩阵、 后验误差相关矩阵
            double[,] data_mTransition = new double[dynam_dims, dynam_dims];
            data_mTransition[0, 0] = 1;
            data_mTransition[0, 1] = 1;
            data_mTransition[1, 0] = 0;
            data_mTransition[1, 1] = 1;

            double[,] data_measure = new double[1, dynam_dims];
            data_measure[0, 0] = 1.0;
            data_measure[0, 1] = 0.0;

            double[,] data_mProcessNoiseCov = new double[dynam_dims, dynam_dims];
            data_mProcessNoiseCov[0, 0] = 0.00001;
            data_mProcessNoiseCov[1, 1] = 0.00001;

            double[,] data_mMeasureNoiseCov = new double[measure_dims, 1];
            data_mMeasureNoiseCov[0, 0] = 0.1;

            double[,] data_mState = new double[dynam_dims, 1];
            double angle = (rand.NextDouble() / 10 - 0.05) * 2; //[-0.1, 0.1]区间分布
            data_mState[0, 0] = angle;
            double angle_inc = (rand.NextDouble() / 10 - 0.05) * 2;
            data_mState[1, 0] = angle_inc;

            double[,] data_mState_post = new double[dynam_dims, 1];
            double angle_post = (rand.NextDouble() / 10 - 0.05) * 2;
            data_mState_post[0, 0] = angle_post;
            double angle_inc_post = (rand.NextDouble() / 10 - 0.05) * 2;
            data_mState_post[1, 0] = angle_inc_post;

            double[,] data_mErrorCov_post = new double[dynam_dims, dynam_dims];
            data_mErrorCov_post[0, 0] = 0.1;
            data_mErrorCov_post[1, 1] = 0.1;

            kalman = new kalmanFilter(dynam_dims, measure_dims);
            kalman._rand = rand;
            kalman.InitKalmanFilter(data_mTransition, data_measure,
                data_mProcessNoiseCov, data_mMeasureNoiseCov, data_mState, data_mState_post, data_mErrorCov_post);

            do
            {
                while (!isPause && !isStop)
                {
                    Thread.Sleep(timeInterval);

                    showBmp = BitmapConvert.ConvertToBitmap(bmSize, PicBackColor);

                    // 1、计算由当前状态确定的点。
                    double state_angle = kalman._mState._MatrixData[0, 0];
                    Point state_Pt = Calc_Point(state_angle, showBmp);

                    // 2、预测kalman模型的状态；并计算由预测状态确定的点。
                    Matrix mState_pre = kalman.KalmanPredict();

                    double statePre_angle = mState_pre._MatrixData[0, 0];
                    Point statePre_Pt = Calc_Point(statePre_angle, showBmp);

                    // 3、通过观测噪声的协方差矩阵，得到当前的观测噪声；并得到观测结果，计算由观测值得到的点
                    Matrix mMeasurementResult = kalman.UpdateMeasurementResult();

                    double measureResult_angle = mMeasurementResult._MatrixData[0, 0];
                    Point measureResult_Pt = Calc_Point(measureResult_angle, showBmp);

                    #region 4、画图
                    Draw_Cross(showBmp, state_Pt, 3, RealPointColor);                   //实际运动点
                    Draw_Cross(showBmp, measureResult_Pt, 3, MeasurePointColor);        //观测点
                    Draw_Cross(showBmp, statePre_Pt, 3, PredictPointColor);             //预测下一运动点

                    Graphics gp = Graphics.FromImage(showBmp);
                    gp.DrawLine(new Pen(Color.Yellow), state_Pt, statePre_Pt);  //在实际运动点与预测点之间连线
                    gp.Dispose();
                    #endregion 4、画图

                    // 5、调节kalman系统模型状态
                    kalman.KalmanCorrect();

                    // 6、通过观测噪声的协方差矩阵，得到当前的观测噪声；并更新系统的运动方程
                    kalman.UpdateState(); ;

                    pictureBoxShow.Image = showBmp;                    
                }
            } while (!isStop);
        }

        private void backworker1_RunWorkerCompleted(object sender, RunWorkerCompletedEventArgs e)
        {
            buttonPause.Enabled = false;

            buttonPicBackColorSel.Enabled = true;
            buttonRealPointColorSel.Enabled = true;
            buttonMeasurePointColorSel.Enabled = true;
            buttonPredictPointColorSel.Enabled = true;
            buttonIndexLineColorSel.Enabled = true;
            textBoxTimeInterval.Enabled = true;
        }

        private void buttonPause_Click(object sender, EventArgs e)
        {
            if (isbackworkerHasWork)
            {
                isPause = !isPause;
                if (isPause)
                {
                    buttonPicBackColorSel.Enabled = true;
                    buttonRealPointColorSel.Enabled = true;
                    buttonMeasurePointColorSel.Enabled = true;
                    buttonPredictPointColorSel.Enabled = true;
                    buttonIndexLineColorSel.Enabled = true;
                    textBoxTimeInterval.Enabled = true;
                    buttonPause.Text = "continue";
                }
                else
                {
                    buttonPicBackColorSel.Enabled = false;
                    buttonRealPointColorSel.Enabled = false;
                    buttonMeasurePointColorSel.Enabled = false;
                    buttonPredictPointColorSel.Enabled = false;
                    buttonIndexLineColorSel.Enabled = false;
                    textBoxTimeInterval.Enabled = false;
                    buttonPause.Text = "pause";
                }
            }
        }

        #region 画图相关函数
        /// <summary>
        /// 根据角度值，计算一个在bmp上以图像中心为中心，以图像宽或高的1/3为半径的点的坐标.
        /// </summary>
        /// <param name="angle"></param>
        /// <returns></returns>
        private Point Calc_Point(double angle, Bitmap bmp)
        {
            int r = Math.Min(bmp.Width, bmp.Height) / 3;

            int x = (int)(bmp.Width / 2 + r * Math.Cos(angle));
            int y = (int)(bmp.Height / 2 - r * Math.Sin(angle));

            return new Point(x, y);
        }

        /// <summary>
        /// 画一个十字叉在Bitmap上
        /// </summary>
        /// <param name="bmp"></图像>
        /// <param name="center"></十字叉的中心点>
        /// <param name="d"></十字叉中直线的长度>
        /// <param name="cl"></十字叉的颜色>
        private void Draw_Cross(Bitmap bmp, Point center, double d, Color cl)
        {
            Graphics gp = Graphics.FromImage(bmp);
            Pen drawPen = new Pen(cl);
            gp.DrawLine(drawPen, new Point((int)(center.X - d), (int)(center.Y - d)), new Point((int)(center.X + d), (int)(center.Y + d)));
            gp.DrawLine(drawPen, new Point((int)(center.X + d), (int)(center.Y - d)), new Point((int)(center.X - d), (int)(center.Y + d)));

            gp.Dispose();
        }
        #endregion 画图相关函数

        #region 界面相关函数
        private void buttonBackColorSel_Click(object sender, EventArgs e)
        {
            ColorDialog dlg = new ColorDialog();
            dlg.CustomColors = _customColors;
            dlg.Color = PicBackColorBox.BackColor;
            if (dlg.ShowDialog() == DialogResult.OK)
            {
                PicBackColorBox.BackColor = dlg.Color;
                PicBackColor = dlg.Color;
                _customColors = dlg.CustomColors;
            }
        }

        private void buttonRealPointColorSel_Click(object sender, EventArgs e)
        {
            ColorDialog dlg = new ColorDialog();
            dlg.CustomColors = _customColors;
            dlg.Color = RealPointColorBox.BackColor;
            if (dlg.ShowDialog() == DialogResult.OK)
            {
                RealPointColorBox.BackColor = dlg.Color;
                RealPointColor = dlg.Color;
                _customColors = dlg.CustomColors;
            }
        }

        private void buttonMeasurePointColorSel_Click(object sender, EventArgs e)
        {
            ColorDialog dlg = new ColorDialog();
            dlg.CustomColors = _customColors;
            dlg.Color = MeasurePointColorBox.BackColor;
            if (dlg.ShowDialog() == DialogResult.OK)
            {
                MeasurePointColorBox.BackColor = dlg.Color;
                MeasurePointColor = dlg.Color;
                _customColors = dlg.CustomColors;
            }
        }

        private void buttonPredictPointColorSel_Click(object sender, EventArgs e)
        {
            ColorDialog dlg = new ColorDialog();
            dlg.CustomColors = _customColors;
            dlg.Color = PredictPointColorBox.BackColor;
            if (dlg.ShowDialog() == DialogResult.OK)
            {
                PredictPointColorBox.BackColor = dlg.Color;
                PredictPointColor = dlg.Color;
                _customColors = dlg.CustomColors;
            }
        }

        private void buttonIndexLineColorSel_Click(object sender, EventArgs e)
        {
            ColorDialog dlg = new ColorDialog();
            dlg.CustomColors = _customColors;
            dlg.Color = IndexLineColortextBox.BackColor;
            if (dlg.ShowDialog() == DialogResult.OK)
            {
                IndexLineColortextBox.BackColor = dlg.Color;
                IndexLineColor = dlg.Color;
                _customColors = dlg.CustomColors;
            }
        }

        private void textBoxTimeInterval_TextChanged(object sender, EventArgs e)
        {
            timeInterval = Convert.ToInt32(textBoxTimeInterval.Text);
        }
        #endregion 界面相关函数
    }
}