准备知识
矩阵的期望:
正交矩阵:如果AATAA^TAAT=E(E为单位矩阵,ATA^TAT表示“矩阵A的转置矩阵”)或ATA=EA^TA=EATA=E,则n阶实矩阵A称为正交矩阵。
协方差矩阵:越线性相关,协方差越大, 两个变量完全线性无关,协方差为零
1、状态空间模型
Xn+1=An+1,nXn+Wn(式1)X_{n+1} = A{n+1},nX_n + W_{n}\tag {式1}Xn+1=An+1,nXn+Wn(式1)
Yn=BnXn+Vn(式2)Y_n = B_nX_n + V_n \tag {式2}Yn=BnXn+Vn(式2)
该模型涉及的参数如下:
(1)状态转移矩阵An+1,nA_{n+1,n}An+1,n,它是可逆的。(在线性代数中,给定一个n阶方阵A,若存在一n阶方阵B使得AB=BA=E(或AB=E、BA=E任满足一个),其中E为n阶单位矩阵,则称A是可逆的,且B是A的逆阵,记作A−1A^{-1}A−1);
(2)测量矩阵BnB_nBn,通常情况下它是长方形矩阵;
(3)高斯动态噪声WnW_nWn,假设它有零均值且有协方差矩阵Qw,nQ_{w,n}Qw,n;
(4)高斯测量噪声VnV_nVn,假设它有零均值且有协方差矩阵Qv,nQ_{v,n}Qv,n;
2、新息过程
处理此类优化估计问题,是利用观察量yny_nyn的所谓新息过程。其定义如下:
an=yn−y^n∣n−1(式3)a_n = y_n-\hat y_{n|n-1} \tag {式3}an=yn−y^n∣n−1(式3)
其中y^n∣n−1\hat y_{n|n-1}y^n∣n−1是在给定至n−1n-1n−1时刻所有观测值的情况下,对yny_nyn的最小均方差的估计。
可以说:
新息过程ana_nan是包含在测量值yny_nyn但不在y^n∣n−1\hat y_{n|n-1}y^n∣n−1的预测部分的新信息的测量,因为yny_nyn可以预测的部分(记为y^n∣n−1\hat y_{n|n-1}y^n∣n−1)是完全由序列{yk}k=1n−1\lbrace y_k\rbrace_{k=1} ^{n-1}{yk}k=1n−1决定的。
新息过程有如下的重要性质:
性质 1:
与观测值yny_nyn有关的新息过程ana_nan与之前的所有观测值y1,y2,...,yn−1y_1,y_2,...,y_{n-1}y1,y2,...,yn−1正交,表示为:
E[anykT]=0,0≤k≤n−1(式4)E[a_n y_k ^T] = 0, 0\leq k \leq n-1 \tag {式4}E[anykT]=0,0≤k≤n−1(式4)
性质 2:
新息过程由一系列相互正交的随机向量构成,表示为:
E[anakT]=0,0≤k≤n−1(式5)E[a_n a_k ^T] = 0, 0\leq k \leq n-1 \tag {式5}E[anakT]=0,0≤k≤n−1(式5)
性质3:
代表观测数据的随机向量序列{y1,y2,....,yn−1}\lbrace y_1,y_2,....,y_{n-1} \rbrace{y1,y2,....,yn−1},与表示更新过程的序列a1,a2,...,ana_1,a_2,...,a_na1,a2,...,an一 一对应。因此,通过能够保证线性稳定并且不丢失任何信息的操作,可以从一个序列得到另一个序列,因此可以写作:
{y1,y2,...,yn} ⟺ {a1,a2,...,an}(式6) \lbrace y_1,y_2,...,y_n\rbrace \iff { \lbrace a_1,a_2,...,a_n \rbrace} \tag {式6}{y1,y2,...,yn}⟺{a1,a2,...,an}(式6)
总结:
该步骤就是给定给n−1n-1n−1时刻所有观测值,然后nnn时刻的观察值做出估计,得到y^n∣n−1\hat y_{n|n-1}y^n∣n−1,得到的估计值与观察值,理论上是存在误差的,定义为新息过程ana_nan。
3、新息过程的协方差矩阵
从初始状态x0x_0x0开始,可以用式1所描述的系统模型表示kkk时刻的系统状态:
Xk=Ak,0X0+∑i=1k−1Ak,iWi(式7)X_k = A_{k,0}X_0 + \sum _{i=1} ^{k-1}A{k,i} W_i \tag {式7}Xk=Ak,0X0+i=1∑k−1Ak,iWi(式7)
式7表明状态XkX_kXk是X0X_0X0以及W1,W2,W3,...,WnW_1,W_2,W_3,...,W_nW1,W2,W3,...,Wn的线性组合。
根据假设,测量噪声VnV_nVn与初始状态X0X_0X0以及动态噪声WiW_iWi无关。因此,在式7两边同时乘以VnTV_n ^TVnT后得到:
E[XkVnT]=0,k,n≥0(式8)E[X_k V_n ^T] = 0 ,k,n \geq 0 \tag{式8}E[XkVnT]=0,k,n≥0(式8)
同理可以从测量公式2得到:
E[YkVnT]=0,0≤k≤n−1(式9)E[Y_k V_n ^T] = 0 ,0 \leq k\leq n-1 \tag{式9}E[YkVnT]=0,0≤k≤n−1(式9)
和
E[YkWnT]=0,0≤k≤n(式10)E[Y_k W_n ^T] = 0 ,0 \leq k\leq n \tag{式10}E[YkWnT]=0,0≤k≤n(式10)
给定先前的观测值y1,...,yky_1,...,y_ky1,...,yk,我们可以从测量公式式2中得出当前的观测值yny_nyn的最小均方估计为:
y^n∣n−1=Bx^n∣n−1+v^n∣n−1(式11)\hat y_{n|n-1} = B \hat x_{n|n-1} + \hat v_{n|n-1} \tag{式11}y^n∣n−1=Bx^n∣n−1+v^n∣n−1(式11)
其中v^n∣n−1\hat v_{n|n-1}v^n∣n−1是给先前的观测值y1,...,yn−1y_1,...,y_{n-1}y1,...,yn−1后所对应的测量噪声估计。因为根据式9,vnv_nvn与先前的观测值是正交的,因此估计值v^n∣n−1\hat v_{n|n-1}v^n∣n−1为零。于是化式11得到:
y^n∣n−1=Bnx^n∣n−1(式12)\hat y_{n|n-1} = B_n \hat x_{n|n-1} \tag{式12}y^n∣n−1=Bnx^n∣n−1(式12)
由于 (yn=bnxn+vny_n = b_nx_n + v_nyn=bnxn+vn),再根据式12代入式3(an=yn−y^n∣n−1a_n = y_n-\hat y_{n|n-1}an=yn−y^n∣n−1),将项合并得:
an=Bnεn∣n−1+vn(式13) a_n = B_n \varepsilon _{n|n-1} + v_n \tag{式13}an=Bnεn∣n−1+vn(式13)
其中,新引入的项 εn∣n−1\varepsilon _{n|n-1}εn∣n−1是状态预测误差向量。其定义为:
εn∣n−1=xn−x^n∣n−1(式14)\varepsilon _{n|n-1} = x_n - \hat x_{n|n-1} \tag{式14}εn∣n−1=xn−x^n∣n−1(式14)
εn∣n−1\varepsilon _{n|n-1}εn∣n−1与动态噪声wnw_nwn以及测量噪声vnv_nvn均是正交的。由此定义零均值新息过程ana_nan的协方差矩阵为:
Rn=E[ananT](式15)R_n = E[a_n a_n ^T]\tag{式15}Rn=E[ananT](式15)
利用式13,能得到:
Rn=BnPn∣n−1BnT+Qn(式16)R_n = B_n P_{n|n-1}B_n^T + Q_n\tag{式16}Rn=BnPn∣n−1BnT+Qn(式16)
其中Qv,nQ_{v,n}Qv,n是测量噪声vnv_nvn的协方差矩阵,新引入的项:
Pn∣n−1=E[εn∣n−1εn∣n−1T](式17)P_{n|n-1} = E[\varepsilon _{n|n-1} \varepsilon ^T_{n|n-1}] \tag{式17}Pn∣n−1=E[εn∣n−1εn∣n−1T](式17)
为预测误差协方差矩阵,式16为卡尔曼滤波算法的第一步。
总结:
4 、利用新息过程进行滤波状态估计:预测-修正公式(X^n∣n=X^n∣n−1+Gnan\hat X_{n|n } = \hat X_{n|n - 1} +G_n a_nX^n∣n=X^n∣n−1+Gnan )
接下来需要做的是利用新息过程实现任意时刻iii系统状态xix_ixi的最小均方误差估计,为此,给定新息序列a1,a2,...,ana_1,a_2,...,a_na1,a2,...,an,首先线性展开的形式表示对状态xix_ixi的估计:
x^i∣n=∑k=1nCi,kak(式18)\hat x _{i|n} = \sum _{k=1}^n C_{i,k} a_k \tag{式18}x^i∣n=k=1∑nCi,kak(式18)
其中{Cj,k}k=1n\lbrace C_{j,k} \rbrace_{k=1} ^n{Cj,k}k=1n是i时刻的展开式系数矩阵的集合,状态预测误差与新息过程满足下述正交条件:
E[εi∣nakT]=0当k=1,2,...,n且i≤n(式19)E[\varepsilon _{i|n} a_k ^T]=0 当k=1,2,...,n且i\leq n \tag{式19}E[εi∣nakT]=0当k=1,2,...,n且i≤n(式19)
因此将式18代入式19,使用式5所描述的信息过程的正交性,可得:
E[xiakT]=Ci,kRk(式20)E[x_i a_k ^T] = C_{i,k}R_k \tag{式20}E[xiakT]=Ci,kRk(式20)
RnR_nRn是新息过程的协方差,解此方程的系数矩阵Ci,kC_{i,k}Ci,k,得到:
Ci,k=E[xiakT]Rk−1(式21)C_{i,k} = E[x_i a_k ^T] R_k ^{-1} \tag{式21}Ci,k=E[xiakT]Rk−1(式21)
再利用式18得到:
x^i∣n=∑k=1nE[xiakT]Rk−1ak(式22)\hat x _{i|n} = \sum _{k=1}^nE[x_i a_k ^T] R_k ^{-1} a_k \tag{式22}x^i∣n=k=1∑nE[xiakT]Rk−1ak(式22)
当i=n时,为滤波过程,因此式26描述该状态的滤波估计为:
x^n∣n=∑k=1nE[xnakT]Rk−1ak=∑k=1n−1E[xnakT]Rk−1ak+E[xnakT]Rk−1ak(式23)\hat x _{n|n} = \sum _{k=1}^nE[x_n a_k ^T] R_k ^{-1} a_k = \sum _{k=1}^{n-1}E[x_n a_k ^T] R_k ^{-1} a_k+ E[x_n a_k ^T] R_k ^{-1} a_k \tag{式23}x^n∣n=k=1∑nE[xnakT]Rk−1ak=k=1∑n−1E[xnakT]Rk−1ak+E[xnakT]Rk−1ak(式23)
把等式的第二项,k=nk=nk=n的项从求和中分离出来,首先将:
x^n∣n−1=∑k=1n−1E[xnakT]Rk−1akv(式24)\hat x _{n|n-1} = \sum _{k=1}^{n-1}E[x_n a_k ^T] R_k ^{-1} a_kv \tag{式24}x^n∣n−1=k=1∑n−1E[xnakT]Rk−1akv(式24)
其次引入下述定义:
Gn=E[xnakT]Rk−1(式25)G_n = E[x_n a_k ^T] R_k ^{-1} \tag{式25}Gn=E[xnakT]Rk−1(式25)
由此我们可以将状态滤波估计表示为下述递归的形式:
x^n∣n=x^n∣n−1+Gnan(式26)\hat x _{n|n} = \hat x _{n|n-1} + G_n a_n \tag{式26}x^n∣n=x^n∣n−1+Gnan(式26)
(1)x^n∣n−1\hat x _{n|n-1}x^n∣n−1 表示单步预测,其表示在给定n−1n-1n−1时刻前所有观测值的基础上对状态xnx_nxn的预测估计。
(2)GnanG_na_nGnan表示修正项,新息过程ana_nan表示由观测值yny_nyn引入滤波过程的新新息,乘以"增益因子"。因此,GnG_nGn通常被称为卡尔曼增益矩阵。
5、卡尔曼增益的计算 Gn=Pn∣n−1BnTRn−1G_n=P_{n|n-1}B_n^TR_n^{-1}Gn=Pn∣n−1BnTRn−1
式26中引入了卡尔曼增益矩阵,需要得到相应的计算公式。
应用式13得:
E[XnanT]=E[Xn(Bnεn∣n−1+vn)T]=E[Xnεn∣n−1T]BnTE[X_na_n^T] = E[X_n(B_n \varepsilon _{n|n-1} +v_n)^T] = E[X_n \varepsilon _{n|n-1}^T]B_n^TE[XnanT]=E[Xn(Bnεn∣n−1+vn)T]=E[Xnεn∣n−1T]BnT
在上式的第二行E[Xn(Bnεn∣n−1+vn)T]E[X_n(B_n \varepsilon _{n|n-1} +v_n)^T]E[Xn(Bnεn∣n−1+vn)T],利用状态XnX_nXn与测量噪声vnv_nvn无关性,根据正交原理,状态预测误差向量εn∣n−1\varepsilon _{n|n-1}εn∣n−1与状态估计X^n∣n−1\hat X_{n|n-1}X^n∣n−1是正交的,因此εn∣n−1\varepsilon _{n|n-1}εn∣n−1与X^n∣n−1\hat X_{n|n-1}X^n∣n−1外积的期望为零,进而我们用εn∣n−1\varepsilon _{n|n-1}εn∣n−1代替XnX_nXn不影响期望E[XnanT]E[X_na_n^T]E[XnanT],由此可得:
E[XnanT]=E[εn∣n−1εn∣n−1T]BnT=Pn∣n−1BnTE[X_na_n^T] = E[\varepsilon _{n|n-1}\varepsilon _{n|n-1}^T]B_n^T=P_{n|n-1}B_n^TE[XnanT]=E[εn∣n−1εn∣n−1T]BnT=Pn∣n−1BnT
因此式25卡尔曼增益矩阵GnG_nGn可以表示为:
Gn=Pn∣n−1BnTRn−1(式27)G_n= P_{n|n-1}B_n^T R_n^{-1} \tag{式27}Gn=Pn∣n−1BnTRn−1(式27)
6、用于更新预测误差协方差矩阵的黎卡堤查分方程
为了解决状态估计,将式14中的n替换为n+1,得到:
εn+1∣n=xn+1−x^n+1∣n\varepsilon _{n+1|n} = x_{n+1} - \hat x_{n+1|n}εn+1∣n=xn+1−x^n+1∣n
通过上式可以看出含有滤波估计的项x^n+1∣n\hat x_{n+1|n}x^n+1∣n表示状态的预测估计是有益的。故而将式24中的n替换为n+1,并运用式1,可得:
x^n+1∣n=∑k=1nE[xn+1akT]Rk−1ak=∑k=1nE[An+1,nXn+WnakT]Rk−1ak=An+1,n∑k=1nE[XnanT]Rk−1ak=An+1,nX^n∣n\hat x_{n+1|n} = \sum_{k=1}^nE[x_{n+1} a_k ^T]R_k^{-1}a_k=\sum _{k=1}^nE[A_{n+1,n}X_n+W_na_k ^T]R_k^{-1}a_k=A_{n+1,n}\sum _{k=1}^nE[X_na_n ^T]R_k ^{-1}a_k=A_{n+1,n}\hat X_{n|n} x^n+1∣n=k=1∑nE[xn+1akT]Rk−1ak=k=1∑nE[An+1,nXn+WnakT]Rk−1ak=An+1,nk=1∑nE[XnanT]Rk−1ak=An+1,nX^n∣n
(式28)\tag{式28}(式28)
因为动态噪声WnW_nWn与观测值是相互独立的,故期望E[wkakT]E[w_ka_k^T]E[wkakT]为零。对滤波估计x^n∣n\hat x_{n|n}x^n∣n,应用式23,以及式28对状态xnx_nxn的预测滤波估计的关系,利用εn+1∣n\varepsilon _{n+1|n}εn+1∣n的公式得到:
εn+1∣n=(An+1,nXn+Wn)−An+1,nX^n∣n=An+1,n(Xn−X^n∣n)+Wn=An+1,nεn∣n+Wn(式29)\varepsilon _{n+1|n} = (A_{n+1,n}X_n+W_n) - A_{n+1,n} \hat X_{n|n} = A_{n+1,n}(X_n-\hat X_{n|n})+W_n=A_{n+1,n} \varepsilon _{n|n} + W_n \tag{式29}εn+1∣n=(An+1,nXn+Wn)−An+1,nX^n∣n=An+1,n(Xn−X^n∣n)+Wn=An+1,nεn∣n+Wn(式29)
滤波误差向量定义为:
εn∣n=Xn−X^n∣n(式30)\varepsilon _{n|n} =X_n-\hat X_{n|n} \tag{式30}εn∣n=Xn−X^n∣n(式30)
因为滤波误差向量εn∣n\varepsilon _{n|n}εn∣n与动态噪声wnw_nwn是无关,可以将预测误差协方差矩阵表示为:
Pn+1,n=E[εn+1∣nεn+1∣nT]=An+1,nPn∣nAn+1T+Qm,n(式31)P_{n+1,n} = E[\varepsilon _{n+1|n}\varepsilon _{n+1|n}^T]=A_{n+1,n}P_{n|n}A_{n+1}^T+Q_{m,n} \tag{式31}Pn+1,n=E[εn+1∣nεn+1∣nT]=An+1,nPn∣nAn+1T+Qm,n(式31)
其中Qm,nQ_{m,n}Qm,n为动态噪声wnw_nwn的误差协方差矩阵。在式30中引入了最后一个参数,称为滤波误差协方差矩阵,其定义为:
Pn∣n=E[εn∣nεn∣nT](式32)P_{n|n}= E[\varepsilon _{n|n} \varepsilon _{n|n}^T] \tag{式32}Pn∣n=E[εn∣nεn∣nT](式32)
为了计算误差协方差矩阵Pn∣nP_{n|n}Pn∣n的式子,因此将式26代入式30得:
εn∣n=Xn−X^n∣n−1−Gnan=εn∣n−1−Gnan\varepsilon _{n|n} = X_n-\hat X_{n|n-1}-G_na_n=\varepsilon _{n|n-1}-G_na_nεn∣n=Xn−X^n∣n−1−Gnan=εn∣n−1−Gnan
然后应用式32,得到:
Pn∣n=E[(εn∣n−1−Gnan)((εn∣n−1−Gnan)T]P_{n|n}=E[(\varepsilon _{n|n-1}-G_na_n)((\varepsilon _{n|n-1}-G_na_n)^T ]Pn∣n=E[(εn∣n−1−Gnan)((εn∣n−1−Gnan)T]
=E[εn∣n−1εn∣n−1T]−GnE[anεn∣n−1T]−E[εn∣n−1anT]GnT+GnE[ananT]GnT=E[\varepsilon _{n|n-1}\varepsilon _{n|n-1}^T]-G_nE[a_n\varepsilon _{n|n-1}^T]-E[\varepsilon _{n|n-1}a_n^T]G_n^T+G_nE[a_na_n^T]G_n^T=E[εn∣n−1εn∣n−1T]−GnE[anεn∣n−1T]−E[εn∣n−1anT]GnT+GnE[ananT]GnT
=Pn∣n−1−GnE[anεn∣n−1T]−E[εn∣n−1anT]+GnRnGnT(式33)=P_{n|n-1}-G_nE[a_n \varepsilon _{n|n-1}^T]-E[ \varepsilon _{n|n-1} a_n^T]+G_nR_nG_n^T \tag{式33}=Pn∣n−1−GnE[anεn∣n−1T]−E[εn∣n−1anT]+GnRnGnT(式33)
因为X^n∣n−1\hat X_{n|n-1}X^n∣n−1与新息过程ana_nan正交,于是得到:
E[εn∣n−1anT]=E[(Xn−X^n∣n−1)anT]=E[XnanT]E[\varepsilon _{n|n-1}a_n^T] = E[(X_n-\hat X_{n|n-1})a_n^T]=E[X_na_n^T]E[εn∣n−1anT]=E[(Xn−X^n∣n−1)anT]=E[XnanT]
同理:
E[anεn∣n−1T]=E[anXnT]E[a_n\varepsilon _{n|n-1}^T]=E[a_nX_n^T]E[anεn∣n−1T]=E[anXnT]
利用式27中对卡尔曼增益的定义,易得:
GnE[anεn∣n−1T]=E[εn∣n−1anT]GnT=GnRnGnTG_nE[a_n\varepsilon _{n|n-1}^T]=E[\varepsilon _{n|n-1}a_n^T]G_n^T=G_nR_nG_n^TGnE[anεn∣n−1T]=E[εn∣n−1anT]GnT=GnRnGnT
根据式33化简得
Pn∣n=Pn∣n−1−GnRnGnTP_{n|n}=P_{n|n-1}-G_nR_nG_n^TPn∣n=Pn∣n−1−GnRnGnT
利用式27以及协方差矩阵RnR_nRn和Pn∣n−1P_{n|n-1}Pn∣n−1的对称性得到:
Pn∣n=Pn∣n−1−GnBnPn∣n−1(式33)P_{n|n}=P_{n|n-1}-G_nB_nP_{n|n-1} \tag{式33}Pn∣n=Pn∣n−1−GnBnPn∣n−1(式33)