卡尔曼滤波是一种高效率的递归滤波器(自回归滤波器)

1、Kalman总述：2API函数名：2运行实例效果：2cvCreateKalman函数：2输入:2输出:3运行逻辑:3输入：4输出：4运行逻辑：4cvKalmanPredict函数：4输入：4输出：4运行逻辑：5cvKalmanCorrect函数：5参数:5运行逻辑:5运行实例：6使用场合：6例子流程：7综合总结：7 卡尔曼滤波是一种高效率的递归滤波器(自回归滤波器), 卡尔曼滤波器把动态系统表示成状态空间形式，根据系统的测量数据（即观测数据）对系统的运动进行估计的。它能够从一系列的不完全包含噪声的测量中，估计动态系统的状态。其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时

2、刻的观测值来更新对状态变量的估计，求出现时刻的估计值。它适合于实时处理和计算机运算。卡尔曼滤波的一个典型实例是从一组有限的，对物体位置的，包含噪声的观察序列预测出物体的坐标位置及速度. 在很多工程应用(雷达, 计算机视觉)中都可以找到它的身影. 同时，卡尔曼滤波也是控制理论以及控制系统工程中的一个重要话题. 比如,在雷达中,人们感兴趣的是跟踪目标,但目标的位置,速度,加速度的测量值往往在任何时候都有噪声.卡尔曼滤波利用目标的动态信息,设法去掉噪声的影响,得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计(滤波),也可以是对于将来位置的估计(预测)，也可以是对过去位置的估计(插值

3、或平滑). 总述： 本文中包含四个kalman 滤波器相关的API函数， API运行效果见实例运行效果。API函数名：cvCreateKalman：分配kalman滤波器结构cvReleaseKalman：释放 Kalman 滤波器结构cvKalmanPredict：估计后来的模型状态cvKalmanCorrect: 调节模型状态运行实例效果：四个API集合在一个实例里面，见实例。cvCreateKalman函数：CvKalman* cvCreateKalman( int DP, int MP, int CP )分配kalman滤波器结构输入:DP : dynamic parameters状态

4、向量维数MP: measure parameters 测量向量维数CP: control parameters 控制向量维数 输出: 无实现运行参数:无运行逻辑:函数 cvCreateKalman 分配 CvKalman 以及它的所有矩阵和初始参数Kalman 结构体如下：typedef struct CvKalman int MP; /dynamic parameters dims状态向量维数int DP;/ measurement parameters dims测量向量维数int CP;/ control parameters dims控制向量维数float* PosterState;/

5、state_pre->data.fl float* PriorState;/ state_post->data.fl float* DynamMatr;/ transition_matrix->data.fl float* MeasurementMatr;/ measurement_matrix->data.fl float* MNCovariance;/ measurement_noise_cov->data.fl float* PNCovariance;/ process_noise_cov->data.fl float* KalmGainMatr;/

6、gain->data.fl float* PriorErrorCovariance;/ error_cov_pre->data.fl float* PosterErrorCovariance;/ error_cov_post->data.fl float* Temp1;/ temp1->data.fl float* Temp2;/ temp2->data.fl CvMat* state_pre;/ 预测状态 (x'(k):x(k)=A*x(k-1)+B*u(k) CvMat* state_post;/ 矫正状态 (x(k): x(k)=x'(k)+

7、K(k)*(z(k)-H*x'(k) CvMat* transition_matrix;/ 状态传递矩阵 state transition matrix (A) CvMat* control_matrix;/ 控制矩阵 control matrix (B) CvMat* measurement_matrix;/ 测量矩阵 measurement matrix (H) CvMat* process_noise_cov;/ 过程噪声协方差矩阵 process noise covariance matrix (Q) CvMat* measurement_noise_cov;/ 测量噪声协方差

8、矩阵 measurement noise covariance matrix (R) CvMat* error_cov_pre;/ 先验误差估计协方差矩阵prior error estimate covariance matrix(P'(k): P'(k)=A*P(k-1)*At + Q)CvMat* gain;/ Kalman 增益矩阵 gain matrix (K(k): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)*/CvMat* error_cov_post;/ 后验误差估计协方差矩阵posterior error estimate c

9、ovariance matrix (P(k): P(k)=(I-K(k)*H)*P'(k) CvMat* temp1;/ 临时矩阵 temporary matrices CvMat* temp2;/ CvMat* temp3;/ CvMat* temp4;/ CvMat* temp5;/CvKalman; cvReleaseKalman函数：void cvReleaseKalman( CvKalman* _kalman )释放 Kalman 滤波器结构输入：kalman: 指向kalman 滤波器结构的双指针输出： 无运行逻辑：释放结构体CvKalman及其所有矩阵cvKalmanPr

10、edict函数：CvMat* cvKalmanPredict( CvKalman* kalman, const CvMat* control )估计后来的模型状态输入：kalman: kalman 滤波器的状态输出：无实际参数选择:Control: 控制向量运行逻辑：函数 cvKalmanPredict 根据当前状态估计后来的随机模型状态，并存储于 kalman->state_pre: , 其中 x'k 是预测状态 (kalman->state_pre), xk 1 是前一步的矫正状态 (kalman->state_post)，应该在开始的某个地方初始化，即缺省为零向

11、量， uk 是外部控制(control 参数), P'k 是先验误差相关矩阵 (kalman->error_cov_pre) Pk 1 是前一步的后验误差相关矩阵(kalman->error_cov_post),应该在开始的某个地方初始化，即缺省为单位矩阵. 函数返回估计得到的状态值 cvKalmanCorrect函数：CvMat* cvKalmanCorrect( CvKalman* kalman, const CvMat* measurement )调节模型状态参数:Kalman: 被更新的 Kalman 结构的指针 Measurement:指向测量向量的指针 运行逻辑:函数 cvKalmanCorrect 在给定的模型状态的测量基础上，调节随机模型状态： 其中 zk - 给定测量(mesurement parameter) Kk - Kalman "增益" 矩阵 函数存储调节状态到 kalman->state_post 中并且输出