基于C#的Kalman滤波器

最新推荐文章于 2024-11-17 11:08:24 发布

csdnbao

最新推荐文章于 2024-11-17 11:08:24 发布

阅读量1w

点赞数

CC 4.0 BY-SA版权

文章标签： c# matrix transition random vector exception

本文链接：https://blog.csdn.net/csdnbao/article/details/4590519

本文介绍了一个在.NET平台上使用C#实现的Kalman滤波器。通过使用CSDN下载频道的C#矩阵库，作者成功地将原本在OpenCV中的CvKalman转换为C#版本，并提供了预测、校正和自动预测等功能。

最近项目用到了kalman滤波器，由于是.NET平台的，原来的OpenCV的那套东西不能用了，还好灵光乍现，开源就是好啊！

花了一个小时将CvKalman用C#进行实现，解决了！

其中的Matrix运算采用了CSDN下载频道的“C#矩阵库”。

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

namespace SimTransfer
{
    public class KalmanFacade
    {
        #region inner class
        class KalmanFilter
        {
            int MP;                     /* number of measurement vector dimensions */
            int DP;                     /* number of state vector dimensions */
            int CP;                     /* number of control vector dimensions */

            public Matrix state_pre;           /* predicted state (x'(k)):
                                        x(k)=A*x(k-1)+B*u(k) */
            public Matrix state_post;          /* corrected state (x(k)):
                                        x(k)=x'(k)+K(k)*(z(k)-H*x'(k)) */
            public Matrix transition_matrix;   /* state transition matrix (A) */
            public Matrix control_matrix;      /* control matrix (B)
                                       (it is not used if there is no control)*/
            public Matrix measurement_matrix; /* measurement matrix (H) */
            public Matrix process_noise_cov;   /* process noise covariance matrix (Q) */
            public Matrix measurement_noise_cov; /* measurement noise covariance matrix (R) */
            public Matrix error_cov_pre;       /* priori error estimate covariance matrix (P'(k)):
                                        P'(k)=A*P(k-1)*At + Q)*/
            Matrix gain;                /* Kalman