卡尔曼滤波大作业

（一）卡尔曼滤波器原理卡尔曼滤波器（KalmanFilter）是一个最优化自回归数据处理算法（optimalrecursivedataprocessingalgorithm）。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。最佳线性滤波理论起源于40年代美国科学家Wiener和前苏联科学家Ｋолмогоров等人的研究工作，后人统称为维纳滤波理论。从理论上说，维纳滤波的最大缺点是必须用到无限过去的数据，不适用于实时处理。为了克服这一缺点，60年代Kalman把状态空间模型引入滤波理论，并导出了一套递推估计算法，后人称之为卡尔曼滤波理论。卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。它适合于实时处理和计算机运算。1.1离散型卡尔曼滤波基本方程首先引入一个离散控制过程的系统：用一个线性随机微分方程来描述：)(WU(k)*B1)-X(k*AX(k)k（1）系统的测量值：V(k)X(k)*HZ(k)（2）X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。A和B是系统参数，对于多模型系统，他们为矩阵。Z(k)是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声，他们的covariance分别是Q，R。利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态：U(k)*B1)-k|1-X(k*A1)-k|X(k（3）式(3)中，X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0。到现在为止，系统结果已经更新了，可是，对应于X(k|k-1)的covariance还没更新。我们用P表示covariance：QA1)-k|1-P(k*A1)-k|P(kT（4）式（4）中，P(k|k-1)是X(k|k-1)对应的covariance，P(k-1|k-1)是X(k-1|k-1)对应的covariance，TA表示A的转置矩阵，Q是系统过程的covariance。式子（3），（4）就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。结合预测值和测量值，我们可以得到现在状态(k)的最优化估算值X(k|k)：1))-k|X(k*H-(Z(k)*Kg(k)1)-k|X(kk)|X(k(5)其中Kg为卡尔曼增益(KalmanGain)：R)H*1)-k|P(k*/(HH*1)-k|P(kKg(k)TT(6)到现在为止，我们已经得到了k状态下最优的估算值X(k|k)。但是为了要另卡尔曼滤波不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k)的covariance：1)-k|P(k*)H*Kg(k)-(Ik)|P(k(7)其中I为1的矩阵，对于单模型单测量，I=1。当系统进入k+1状态时，P(k|k)就是式子(4)的P(k-1|k-1)。这样，算法就可以自回归的运算下去。1.2离散型卡尔曼滤波基本方程的直观推导1.一步预测方程推导一步预测是根据k-1时刻的状态估计预测k时刻的状态，即根据k-1个量测121,,kZZZ对kX作线性最小方差估计：],|))(W1)-X(k*A[(],|[)1|(121*121*KKkZZZkEZZZXEKKX根据最小方差估计的线性1，有：],|)(W[*],|[A)1|(ˆ1211211*KKkZZZkEZZZXEKKXW（k）只影响X（k），所以W（k-1）与121,KZZZ不相关，且0)]([*kWE，所以：0],|)(W[*121KZZZkE而11211*ˆ],|[kKkXZZZXE因此：)1(ˆ*)1|(ˆkXAKKX2.状态估计方程推导用一步预测代替真实值X（k）引起的误差为：)1|(ˆ)()1|(~kkXkXkkX引起对量测的估计误差：)1|(ˆ)()1|(~kkZkZkkZ=)1|(ˆ*)()(*kkXHkVkXH=)()1|(~*kVkkXH滤波理论中称)1|(~kkZ为残差(新息)。从上市可以看出，残差里面包含了进一步预测的误差信息，对)1|(~kkZ作适当加权处理就能得到)1|(~kkX，用来修正)1|(ˆkkX即可得到状态的估计：))1|(ˆ*)()(()1|(ˆ)1|(~)()1|(ˆ)(ˆkkXHkZkKkkXkkZkKkkXkX)(Kk为对残差的加权矩阵，称为滤波增益矩阵。3.滤波增益矩阵及估计均方误差矩阵推导增益矩阵的选取是使))(~)(~()(kXkXEKPT最小，而)))1|(ˆ*)()(()1|(ˆ()()(~kkXHkZkKkkXKXkX=)(*)()1|(~**)()1|(~kVkKkkXHkKkkX=)(*)()1|(~*)*)((kVkKkkXHkKI所以：))(~)(~()(kXkXEKPT=})](*)()1|(~*)*)())][((*)()1|(~*)*)({[(TkVkKkkXHkKIkVkKkkXHkKIE=TTHkKIkkXkkXEHkKI]*)(}[)]1|(~)][1|(~{[)*)((+)()*()(kKVVEkKT-TTKVkkXEHkKI))1|(~()*)((-TTHkKIkkXkVEK)*)())}(1|(~)({*由于)1|(ˆkkX是跟据k-1时刻前的量测对K时刻的状态所作的估计，而V是量测的噪声，所以V与)1|(~kkX不相关并且][VE=0，因此：0})1|(~{))}1|(~)({TTVkkXEkkXkVE带入上式：TTHkKIkkXkkXEHkKIKP]*)(}[)]1|(~)][1|(~{[)*)(()(+)(**)(kKRkKT式中：})]1|(~)][1|(~{[)1|(TkkXkkXEkkP)*(TVVER下面根据极值原理推导出滤波增益矩阵K。设K是使估计的均方误差阵达到最小的最佳增益矩阵，并设该最小误差矩阵为P，显然，若滤波增益矩阵偏离最佳增益矩阵的偏移量为δK，则估计的均方误差将偏移最小值P，而达到P+δP，且δP为非负定矩阵，及δP0，K+δK和P+δP，满足方程：TkHkKkKIkkPkHkKkKIkPkP)]())()(()[1|()]())()(([)()(+TkKkKRkKkK)]()([**))()((将P(K)用基本方程替换得：)())()1|()()(()(kKRkHkkPkHkKWWkPTkTT式中：)]()())()()(1|()[(kKkRkKkHIkkPHkKWTTTk=)]())()1|(()1|()[(kKkRkkPHkkPHkKTkk若取0)())()1|((kKkRkkPHHTkk及1))()1|()(()1|()(kRkkPHkHkkPkKkT则：W=0)())()()1|()()(()(kKkRkHkkPkHkKkPTT由于R（k）为正定矩阵，P（k|k-1）至少为非负定矩阵，)()()1|()(kRkHkkPkHT至少为非负定矩阵。若)(kK为非零矩阵，)(kP至少为非负定矩阵，即0)(kP。这说明，)(kK按上面式子计算，则对于相对增益矩阵)(kK的任何偏移0)(kK，估计的均方误差将产生非负的偏差)(kP，因此)(kK是使估计的均方误差最小的最佳增益矩阵。4.一步预测均方误差矩阵的推导一步预测产生的误差为WkXAkkXkXkkX)1(~*)1|(ˆ)()1|(~所以一步预测均方差矩阵为：))1|(~)1|(~()1|(kkXkkXEkkPT=}])(~*][)(~*{[TWkXAWkXAE由于W与1)-(kX~不相关又0)(wE所以：)1()1(*)1|(kQAkPAkkPT至此基本公式推导完毕！（二）卡尔曼滤波应用实例：目标追踪问题1.opencv中KalmanFilter应用opencv给出了kalmanfilter的一个实现，即：kalmanfilter类。opencv中kalmanfilter保留的接口很简单，只有三个：init，predict及correct。init仅仅是初始化卡曼滤波的维数；predict就能得到估计的结果；correct要将测量结果作为输入，得到最优化的估计，并为下次predict做准备。除此之外，kalmanfilter类中还有10个比较复杂的参数，它们与kalmanfilter的原理密切相关。5个矩阵是基本参数：MattransitionMatrix;//转移矩阵MatcontrolMatrix;//控制矩阵MatmeasurementMatrix;//观测矩阵MatprocessNoiseCov;//系统过程二阶协方差矩阵MatmeasurementNoiseCov;//测量过程二阶协方差矩阵5个矩阵是kalmanfilter公式计算的结果MatstatePre;//)1|(ˆkkXMatstatePost;//)(ˆkXMaterrorCovPre;//)1|(kkPMatgain;//)(kKMaterrorCovPost;//)|1(kkP在使用kalmanfilter之前，需要初始化5个基本参数。重点关注transitionMatrix；processNoiseCov；measurementNoiseCov；measurementMatrix：controlMatrix。errorCovPost需要初始化，当其为1时，比为0时，收敛快的多。由公式可以看出。statePost初始化尽量接近下一个状态值，如果收敛很快，这个不是太重要。2.程序代码//OpenCVTest.cpp:定义控制台应用程序的入口点。#includestdafx.h#includeopencv2/opencv.hppusingnamespacestd;usingnamespacecv;#includecv.h#includecxcore.h#includehighgui.h#includecmath#includevector#includeiostreamconstintwinHeight=600;constintwinWidth=800;CvPointmousePosition=cvPoint(winWidth1,winHeight1);//mouseeventcallbackvoidmouseEvent(intevent,intx,inty,intflags,void*param){if(event==CV_EVENT_MOUSEMOVE){mousePosition=cvPoint(x,y);}}intmain(void){//1.kalmanfiltersetupconstintstateNum=4;constintmeasureNum=2;CvKalman*kalman=cvCreateKalman(stateNum,measureNum,0);//state(x,y,detaX,detaY)CvMat*process_noise=cvCreateMat(stateNum,1,CV_3