过程方程:
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)和~Z(k+1|k)都是单列矩阵,因此不难看出,满足式8的矩阵K应有无穷多个。矩阵K中第i行第j列反映了量测变量偏差矩阵~Z(k+1|k)的第j个元素对状态变量偏差矩阵~X(k+1|k)的第i个元素的贡献。因此矩阵K的物理意义很明显,K的第i行第j列的元素表示:对于第i个待测的状态量来说,第j个测量仪器测到的偏差的可信度。某个测量值对应的可信度越高,滤波器越“相信”该测量值。
既然满足条件的K有无穷多个,那应该使用哪个K呢?实际上,我们并不知道~X(k+1|k)的值,所以也就无法直接计算出K,而只能通过某种方法找到一个Kg,使得将Kg带入式8后,等号两边的差(的平方)的期望尽可能小。
我们最终的预测值或滤波值是后验预测值^X(k+1|k+1),因此最后的预测也应使 ~X(k+1|k+1) 的期望为0且方差最小(这与让8式两端的差最小是一致的,下面的式9体现了这一点),这样预测值才最可靠。下面详细说明。
^X(k+1|k+1) = ^X(k+1|k) + Kg * ~Z(k+1|k) (后验预测的状态值)
~X(k+1|k+1) = X(k+1) - ^X(k+1|k+1) (后验预测的偏差)
~X(k+1|k+1) = X(k+1) - ^X(k+1|k+1)
= ( ^X(k+1|k) + ~X(k+1|k) ) - ( ^X(k+1|k) + Kg * ~Z(k+1|k) )
= ~X(k+1|k) - Kg * ~Z(k+1|k)
>>>>式9
~Z(k+1|k) = Z(k+1) - ^Z(k+1|k)
= ( H X(k+1)+ V(k+1) ) - ( H ^X(k+1|k) )
= H ( X(k+1)-^X(k+1|k) ) + V(k+1)
= H ~ X(k+1|k) + V(k+1) >>>>式10
接下来的分析中,为了更直观的说明卡尔曼滤波的原理,我们用几何方法来解释。这时,~X和~Z矩阵中的每个元素应看做向量空间中的一个向量而不再是一个单纯的数。这个向量空间(统计测试空间)可以看成无穷多维的,每一个维对应一个可能的状态。~X和~Z矩阵中的每个元素向量都是由所有可能的状态按照各自出现的概率组合而成(在测量之前,~X和~Z 的实际值都是不可知的)。~X和~Z中的每个元素向量都应是0均值的,他们与自己的内积就是他们的协方差矩阵。我们无法给出~X和~Z中每个元素向量的具体表达,但我们通过协方差矩阵就可以知道所有元素向量的模长,以及相互之间的夹角(从内积计算)。
为了方便用几何方法解释,我们假设状态变量X是一个1行1列的矩阵(即只有一个待测状态量),而量测变量Z是一个2行1列的矩阵(即有两个测量仪器,共同测量同一个状态量X),也就是说,m=1,n=2。矩阵X中只有X[1]一项,矩阵Z中有Z[1]和Z[2]两项。Kg此时
补充:综合编程 , 其他综合 ,