最近项目用到了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'(