卡尔曼滤波原理.doc

过程方程:
X(k+1) =  A X(k) + B U(k) + W(k)               >>>>式1
量测方程:
Z(k+1) =  H X(k+1)+ V(k+1)                  >>>>式2
A和B是系统参数,对于多模型系统,他们为矩阵;H是测量系统的参数,对于多测量系统,H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声,他们的协方差分别是Q,R。为了不失一般性,下面的讨论中将X,Z都视为矩阵,其中X是m行的单列矩阵,Z是n行的单列矩阵。
 
说明:下面的表达式中,不带前缀的量都代表实际量,其小括号里面的“k”或“k+1”代表该量是第k或第k+1时刻的实际量,如“Z(k+1)”就代表第k+1时刻的实际测量值;
带前缀“^”的量都代表预测量,如果小括号里面是“k+1|k”,就代表k+1时刻的先验预测值,如果小括号里面是“k+1|k+1”,就代表k+1时刻的后验预测值;(测量值可以通过测量得到,所以只有先验预测,没有后验预测。而实际状态值无法得知,既有先验预测,又有后验预测)
带前缀“~”的量都代表与预测值对应的偏差值。
 
实际状态值与先验预测状态值的偏差= 实际状态值–先验预测状态值
~X(k+1|k)      =     X(k+1)    -      ^X(k+1|k)             >>>> 式3
 
实际测量值与先验预测测量值的偏差= 当前测量值- 先验预测测量值
~Z(k+1|k)  = Z(k+1)  -   ^Z(k+1|k)                                   >>>>式4
 
 
并且
先验预测测量值  =  转换矩阵H * 先验预测状态值
^Z(k+1|k) =  H ^X(k+1|k)                            >>>> 式5
 
得到测量值后,再对当前状态值X(k+1) 进行后验预测(设后验预测值为^Z(k+1|k+1) ) ,则后验预测值(同时也是最终预测值)的偏差为
~X(k+1|k+1)  =     X(k+1)    -      ^X(k+1|k+1)                 >>>>式6
 
 
为了得到当前状态值X(k+1), 根据式3,需要:
X(k+1) =  ^X(k+1|k)  + ~X(k+1|k)                        >>>> 式7
上式中,我们可以通过卡尔曼公式1(见附注2)计算出^X(k+1|k),但我们无法得知实际状态值X(k+1),因而~X(k+1|k) 也无法得知。我们最终的目的是得出一个比较接近实际状态值X(k+1)的滤波值^X(k+1|k+1),根据式7,只要能准确的估计出~X(k+1|k)即可。
~X(k+1|k)本身虽无法得知,但~Z(k+1|k) 却可以通过测量得到,而且它们二者存在一定的相关性。不妨再设存在一个矩阵K(m行n列矩阵),能使得
 ~X(k+1|k) = K * ~Z(k+1|k)                                        >>>>式8
那么最终的预测任务其实就是找到K。由于~X(k+1|k)和~