kalmanfilter.cs

在Visual 2008环境下

/////////////////////////////////////////////////////////////////////////////////////////////////////
//  注：此卡尔曼滤波器仿照《OpenCV教程 基础篇》
//此类仍存在的一个问题是，Random类不能产生满足卡尔曼滤波器所要求(在一定均值、一定方差下)的正态分布的噪声。
//  by zhd, 2008.12.5
/////////////////////////////////////////////////////////////////////////////////////////////////////
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

class kalmanFilter
{
    #region Field
    /// <summary>
    /// 随机数产生
    /// </summary>
    private Random rand = null;

    /// <summary>
    /// 状态向量维数
    /// </summary>
    private int dynam_params;

    /// <summary>
    /// 测量(结果)向量维数
    /// </summary>
    private int measure_parms;

    /// <summary>
    /// 状态转移矩阵
    /// 在kalman模型状态的估计中、系统运动方程中计算运动噪声向量时用到
    /// </summary>
    private Matrix mTransition = null;

    /// <summary>
    /// 观测矩阵
    /// 在kalman模型状态的调整中、系统观测方程中计算观测噪声向量时用到
    /// </summary>
    private Matrix mMeasurement = null;

    /// <summary>
    /// 过程噪声的协方差矩阵
    /// 在kalman模型状态的估计中、系统运动方程中计算运动噪声向量时用到——同mTransition
    /// </summary>
    private Matrix mProcessNoiseCov = null;

    /// <summary>
    /// 观测噪声的协方差矩阵
    /// 在kalman模型状态的调整中、系统观测方程中计算观测噪声向量时用到——同mMeasurement
    /// </summary>
    private Matrix mMeasureNoiseCov = null;

    /// <summary>
    /// 测量结果矩阵
    /// 1.在系统观测方程更新结果
    /// 2.在kalman模型状态的调整中用到
    /// </summary>
    private Matrix mMeasurementResult = null;

    #region 三种状态矩阵
    /// <summary>
    /// 状态矩阵
    /// 1.在系统运动方程更新结果
    /// 2.在系统观测方程中用到
    /// </summary>
    private Matrix mState = null;

    /// <summary>
    /// 先验(or预测)状态矩阵
    /// 1.在kalman模型状态的估计中更新结果
    /// 2.在kalman模型状态的调整中用到
    /// </summary>
    private Matrix mState_pre = null;

    /// <summary>
    /// 后验状态矩阵
    /// 1.在kalman模型状态的调整中更新结果
    /// 2.在kalman模型状态的估计中用到
    /// </summary>
    private Matrix mState_post = null;
    #endregion 三种状态矩阵

    #region 两种误差相关矩阵
    /// <summary>
    /// 先验误差相关矩阵
    /// 1.在kalman模型状态的估计中更新结果
    /// 2.在kalman模型状态的调整中用到
    /// </summary>
    private Matrix mErrorCov_pre = null;

    /// <summary>
    /// 后验误差相关矩阵
    /// 1.在kalman模型状态的调整中更新结果
    /// 2.在kalman模型状态的估计中用到
    /// </summary>
    private Matrix mErrorCov_post = null;

    #endregion 两种误差相关矩阵

    /// <summary>
    /// 卡尔曼增益矩阵
    /// </summary>
    private Matrix mKalmanGain = null;
    #endregion Field

    #region Property
    public Matrix _mState
    {
        get
        {
            return mState;
        }
    }
    public Random _rand
    {
        get
        {
            return rand;
        }
        set
        {
            rand = value;
        }
    }
    #endregion Property

    #region Constructor
    /// <summary>
    /// 卡尔曼滤波器的构造函数
    /// </summary>
    /// <param name="dynam_dims"></状态向量维数>
    /// <param name="measure_dims"></测量(结果)向量维数>
    public kalmanFilter(int dynam_dims, int measure_dims)
    {
        dynam_params = dynam_dims;
        measure_parms = measure_dims;

        #region 设置相关矩阵维数
        mTransition = new Matrix(dynam_params, dynam_params);
        mMeasurement = new Matrix(measure_parms, dynam_params);
        mProcessNoiseCov = new Matrix(dynam_params, dynam_params);
        mMeasureNoiseCov = new Matrix(measure_parms, measure_parms);

        mMeasurementResult = new Matrix(measure_parms, 1);
        mState = new Matrix(dynam_params, 1);
        mState_pre = new Matrix(dynam_params, 1);
        mState_post = new Matrix(dynam_params, 1);

        mErrorCov_pre = new Matrix(dynam_params, dynam_params);
        mErrorCov_post = new Matrix(dynam_params, dynam_params);

        mKalmanGain = new Matrix(dynam_params, measure_parms);
        #endregion 设置相关矩阵维数

        rand = new Random();
    }
    #endregion Constructor

    #region Method
    /// <summary>
    /// 初始化卡尔曼滤波器，设置一些矩阵的初始值
    /// </summary>
    /// <param name="Data_Transition"></状态转移矩阵>
    /// <param name="Data_Measure"></观测矩阵>
    /// <param name="Data_ProNoiseCov"></过程噪声的协方差矩阵>
    /// <param name="Data_MeaNoiseCov"></观测噪声的协方差矩阵>
    /// <param name="Data_State"></状态矩阵>
    /// <param name="Data_State_post"></后验状态矩阵>
    /// <param name="Data_ErrorCov_post"></后验误差相关矩阵>
    public void InitKalmanFilter(double[,] Data_Transition, double[,] Data_Measure,
        double[,] Data_ProNoiseCov, double[,] Data_MeaNoiseCov,
        double[,] Data_State, double[,] Data_State_post, double[,] Data_ErrorCov_post)
    {
        mTransition._MatrixData = Data_Transition;
        mMeasurement._MatrixData = Data_Measure;
        mProcessNoiseCov._MatrixData = Data_ProNoiseCov;
        mMeasureNoiseCov._MatrixData = Data_MeaNoiseCov;

        mState._MatrixData = Data_State;
        mState_post._MatrixData = Data_State_post;
        mErrorCov_post._MatrixData = Data_ErrorCov_post;       
    }

    /// <summary>
    /// 更新系统的运动方程
    /// </summary>
    /// <returns></更新后的状态矩阵>
    public Matrix UpdateState()
    {
        #region 通过观测噪声的协方差矩阵，得到当前的观测噪声

        double[,] data_wk = new double[dynam_params, 1];
        for (int i = 0; i < dynam_params; i++)
        {
            double processNoise = Math.Sqrt(mProcessNoiseCov._MatrixData[i, i]);
            double temp_processNoise = rand.NextDouble() * processNoise * 2 - processNoise;
            data_wk[i, 0] = temp_processNoise;
        }

        Matrix wk = new Matrix(2, 1);
        wk._MatrixData = data_wk;

        #endregion 通过观测噪声的协方差矩阵，得到当前的观测噪声

        mState = mTransition * mState + wk;

        return mState;
    }

    /// <summary>
    /// 更新系统的观测方程
    /// </summary>
    /// <returns></更新后的测量结果矩阵>
    public Matrix UpdateMeasurementResult()
    {
        #region 通过观测噪声的协方差矩阵，得到当前的观测噪声

        double[,] data_vk = new double[measure_parms, 1];
        for (int i = 0; i < measure_parms; i++)
        {
            double measureNoise = Math.Sqrt(mMeasureNoiseCov._MatrixData[i, i]);
            double temp_measureNoise = rand.NextDouble() * measureNoise * 2 - measureNoise;
            data_vk[i, 0] = temp_measureNoise;
        }

        Matrix vk = new Matrix(measure_parms, 1);
        vk._MatrixData = data_vk;

        #endregion 通过观测噪声的协方差矩阵，得到当前的观测噪声

        mMeasurementResult = mMeasurement * mState + vk;

        return mMeasurementResult;
    }

    /// <summary>
    /// 进行kalman预测
    /// </summary>
    /// <returns></更新后的先验(or预测)状态矩阵>
    public Matrix KalmanPredict()
    {
        mState_pre = mTransition * mState_post;

        Matrix mTransition_tranpose = mTransition.MatrixTranspose();
        mErrorCov_pre = mTransition * mErrorCov_post * mTransition_tranpose + mProcessNoiseCov;

        return mState_pre;
    }

    /// <summary>
    /// 调节kalman模型状态
    /// </summary>
    /// <returns></更新后的后验状态矩阵>
    public Matrix KalmanCorrect()
    {
        Matrix mMeasurement_tranpose = mMeasurement.MatrixTranspose();
        Matrix temp = mMeasurement * mErrorCov_pre * mMeasurement_tranpose + mMeasureNoiseCov;
        temp.MatrixInvert();
        mKalmanGain = mErrorCov_pre * mMeasurement_tranpose * temp;

        mState_post = mState_pre + mKalmanGain * (mMeasurementResult - mMeasurement * mState_pre);

        Matrix mIdentity = new Matrix(mErrorCov_post._Row, mErrorCov_post._Col);
        mErrorCov_post = (mIdentity - mKalmanGain * mMeasurement) * mErrorCov_pre;

        return mState_post;
    }
    #endregion Method
}