卡尔曼滤波简述以及代码

@[TOC]卡尔曼滤波

卡尔曼滤波概述

卡尔曼滤波适用于估计一个动态系统的最优状态。即便是观测到的系统状态参数含有噪声，观测值不准确，卡尔曼滤波也能够完成对状态真实值的最优估计。

卡尔曼滤波基本公式

滤波估计方程（KK时刻的最优值）：
xk=xk/k−1+Bk−1uk−1+Jk−1Zk−1+Hk[Zk−CK∗xk/k−1−Ck∗Bk−1uk−1]
xk=xk/k−1+Bk−1uk−1+Jk−1Zk−1+Hk[Zk−CK∗xk/k−1−Ck∗Bk−1uk−1]
xkxk为kk时刻最优值，
xk/k−1xk/k−1为根据k−1k−1时刻进行预测得到的kk时刻的预测值，由状态一步预测方程得到；
uk−1uk−1是控制量，Bk−1Bk−1为k−1k−1时刻控制系数矩阵
Jk−1Zk−1Jk−1Zk−1分别是过程激励噪声，可以简单理解为噪声
状态一步预测方程
xk/k−1=Ak,k−1xk−1+Bk−1uk−1+Jk−1Zk−1
xk/k−1=Ak,k−1xk−1+Bk−1uk−1+Jk−1Zk−1
uk−1uk−1是控制量
均方误差一步预测：
Pk/k−1=Ak,k−1∗Pk−1∗ATk,k−1+Tk−1Qk−1TTk−1−Jk−1STk−1TTk−1
Pk/k−1=Ak,k−1∗Pk−1∗Ak,k−1T+Tk−1Qk−1Tk−1T−Jk−1Sk−1TTk−1T
Tk−1Qk−1TTk−1−Jk−1STk−1TTk−1Tk−1Qk−1Tk−1T−Jk−1Sk−1TTk−1T为观测噪声，
QkQk为过程噪声的协方差，其为非负定矩阵；
均方误差更新矩阵（K时刻最优均方误差）：
Pk=[I−Hk∗Ck]∗Pk/k−1
Pk=[I−Hk∗Ck]∗Pk/k−1
滤波增益方程（权重系数更新）：
Hk=Pk/k−1∗CTk[Ck∗Pk/k−1∗CTk+Rk]−1
Hk=Pk/k−1∗CkT[Ck∗Pk/k−1∗CkT+Rk]−1

其中，RkRk为测量噪声的协方差，其为正定矩阵；

代码

float KalmanFilter(float inData) 
{  
	static float prevData=0;   
	static float p=10, q=0.001, r=0.001, kGain=0;    
	p = p+q;     
	kGain = p/(p+r);    
	inData = prevData+(kGain*(inData-prevData));     
	p = (1-kGain)*p;   
	prevData = inData;   
	return inData; 
}