从贝叶斯估计到卡尔曼滤波(详细推导)(下)
文章目录
-
- 二.高斯加权分布的多维积分运算
-
- 1.第一章总结
- 2.修正卡尔曼滤波器更新方程的推导过程
- 3.高斯密度下贝叶斯点预测的积分运算
- 三.线性卡尔曼滤波器
-
- 1.线性状态方程
- 2.线性观测方程
- 3.总结
- 尾声
承接上文 从贝叶斯估计到卡尔曼滤波(详细推导)(上)
二.高斯加权分布的多维积分运算
1.第一章总结
现在我们针对上一章中的重要结果做一个简单回顾与总结。
我们假设在一个动态过程，其形式满足下式
x n = f n − 1 ( x n − 1 ) + u n + v n − 1 ( 42 ) x_{n}=f_{n-1}(x_{n-1})+u_{n}+v_{n-1}(42) xn=fn−1(xn−1)+un+vn−1(42)
并且观测过程满足
z n = h n ( x n ) + w n ( 43 ) z_{n}=h_{n}(x_{n})+w_{n}(43) zn=hn(xn)+wn(43)
基于到 t n − 1 t_{n-1} tn−1时刻为止的所有观测值，对 t n t_{n} tn时刻状态向量的预测估计可以写为
x ^ n ∣ n − 1 = ∫ [ f n − 1 ( x n − 1 ) + u n + v n − 1 ] p ( x n − 1 ∣ z 1 : n − 1 ) d x n − 1 ( 44 ) \hat x_{n|n-1}=\int \left [f_{n-1}(x_{n-1})+u_{n}+v_{n-1}\right ]p(x_{n-1}|z_{1:n-1})dx_{n-1}(44) x^n∣n−1=∫[fn−1(xn−1)+un+vn−1]p(xn−1∣z1:n−1)dxn−1(44)
对该状态向量的协方差矩阵的预测估计可以由下式得到
P n ∣ n − 1 x x = ∫ [ f n − 1 ( x n − 1 ) + u n − x ^ n ∣ n − 1 ] [ f n − 1 ( x n − 1 ) + u n − x ^ n ∣ n − 1 ] T p ( x n − 1 ∣ z 1 : n − 1 ) d x n − 1 + Q ( 45 ) P^{xx}_{n|n-1}=\int \left [f_{n-1}(x_{n-1})+u_{n}-\hat x_{n|n-1}\right ]\left [f_{n-1}(x_{n-1})+u_{n}-\hat x_{n|n-1}\right ]^{T}p(x_{n-1}|z_{1:n-1})dx_{n-1}\\ +Q(45) Pn∣n−1xx=∫[fn−1(xn−1)+un−x^n∣n−1][fn−1(xn−1)+un−x^n∣n−1]Tp(xn−1∣z1:n−1)dxn−1+Q(45)
另外，对观测向量 z n z_{n} zn、 z n z_{n} zn的协方差矩阵， z n z_{n} zn与 x n x_{n} xn的互协方差矩阵的预测估计由下式得到
z ^ n ∣ n − 1 = ∫ h n ( x n ) p ( x n ∣ z 1 : n − 1 ) d x n + ∫ w n p ( x n ∣ z 1 : n − 1 ) d x n ( 46 ) \hat z_{n|n-1}=\int h_{n}(x_{n})p(x_{n}|z_{1:n-1})dx_{n}\\+\int w_{n}p(x_{n}|z_{1:n-1})dx_{n}(46) z^n∣n−1=∫hn(xn)p(xn∣z1:n−1)dxn+∫wnp(xn∣z1:n−1)dxn(46)
P n ∣ n − 1 z z = ∫ [ h n ( x n ) − z ^ n ∣ n − 1 ] [ h n ( x n ) − z ^ n ∣ n − 1 ] T × p ( x n ∣ z 1 : n − 1 ) d x n + R ( 47 ) P^{zz}_{n|n-1}=\int [h_{n}(x_{n})-\hat z_{n|n-1}][h_{n}(x_{n})-\hat z_{n|n-1}]^{T}\\ \times p(x_{n}|z_{1:n-1})dx_{n}+R(47) Pn∣n−1zz=∫[hn(xn)−z^n∣n−1][hn(xn)−z^n∣n−1]T×p(xn∣z1:n−1)dxn+R(47)
P n ∣ n − 1 x z = ∫ [ x n − x ^ n ∣ n − 1 ] [ h n ( x n ) − z ^ n ∣ n − 1 ] T × p ( x n ∣ z 1 : n − 1 ) d x n ( 48 ) P^{xz}_{n|n-1}=\int [x_{n}-\hat x_{n|n-1}][h_{n}(x_{n})-\hat z_{n|n-1}]^{T}\\ \times p(x_{n}|z_{1:n-1})dx_{n}(48) Pn∣n−1xz=∫[xn−x^n∣n−1][hn(xn)−z^n∣n−1]T×p(xn∣z1:n−1)dxn(48)
其中，
R : = ∫ w n w n T p ( x n ∣ z 1 : n − 1 ) d x n ( 49 ) R:=\int w_{n}w_{n}^{T}p(x_{n}|z_{1:n-1})dx_{n}(49) R:=∫wnwnTp(xn∣z1:n−1)dxn(49)
Q : = ∫ v n − 1 v n − 1 T p ( x n − 1 ∣ z 1 : n − 1 ) d x n − 1 ( 50 ) Q:=\int v_{n-1}v_{n-1}^{T}p(x_{n-1}|z_{1:n-1})dx_{n-1}(50) Q:=∫vn−1vn−1Tp(xn−1∣z1:n−1)dxn−1(50)
2.修正卡尔曼滤波器更新方程的推导过程
在这一节，我们假设所有的条件密度都是高斯的，并在次假设下重新推导卡尔曼的更新方程。
根据贝叶斯准则，可以得到
p ( x n ∣ z 1 : n ) = p ( x n , z n ∣ z 1 : n − 1 ) p ( z n ) ( 51 ) p(x_{n}|z_{1:n})=\frac{p(x_{n},z_{n}|z_{1:n-1})}{p(z_{n})}(51) p(xn∣z1:n)=p(zn)p(xn,zn∣z1:n−1)(51)
现在定义一个联合向量
q : = [ x n z n ] ( 52 ) q:=\begin{bmatrix} x_{n}\\ z_{n} \end{bmatrix}(52) q:=[xnzn](52)
则式(51)可被重写为
p ( x n ∣ z 1 : n ) = p ( q n ∣ z 1 : n − 1 ) p ( z n ) ( 53 ) p(x_{n}|z_{1:n})=\frac{p(q_{n}|z_{1:n-1})}{p(z_{n})}(53) p(xn∣z1:n)=p(zn)p(qn∣z1:n−1)(53)
我们知道，多元高斯分布的一般形式可写为
N ( t ; s , Σ ) : = 1 ( 2 π ) n ∣ Σ ∣ e x p { − 1 2 ( t − s ) T Σ − 1 ( t − s ) } ( 54 ) N(t;s,\Sigma ):=\frac{1}{\sqrt{(2\pi)^{n}|\Sigma|}}exp\left \{-\frac{1}{2}(t-s)^{T}\Sigma^{-1}(t-s)\right \}(54) N(t;s,Σ):=(2π)n∣Σ∣1exp{−21(t−s)TΣ−1(t−s)}(54)
则式(52)中的概率密度可以表示为
p ( x n ∣ z 1 : n ) = N ( x n ; x ^ n ∣ n , P n ∣ n x x ) ( 55 ) p(x_{n}|z_{1:n})=N(x_{n};\hat x_{n|n},P_{n|n}^{xx})(55) p(xn∣z1:n)=N(xn;x^n∣n,Pn∣nxx)(55)
p ( q n ∣ z 1 : n − 1 ) = N ( q n ; q ^ n ∣ n − 1 , P n ∣ n q q ) ( 56 ) p(q_{n}|z_{1:n-1})=N(q_{n};\hat q_{n|n-1},P_{n|n}^{qq})(56) p(qn∣z1:n−1)=N(qn;q^n∣n−1,Pn∣nqq)(56)
p ( z n ) = N ( z n ; z ^ n ∣ n − 1 , P n ∣ n − 1 z z ) ( 57 ) p(z_{n})=N(z_{n};\hat z_{n|n-1},P_{n|n-1}^{zz})(57) p(zn)=N(zn;z^n∣n−1,Pn∣n−1zz)(57)
忽略系数 − 1 2 -\frac{1}{2} −21,则式(53)的左式中，后验概率密度的解析表达式中的指数项的指数可以写为
d : = ( x n − x ^ n ∣ n ) T [ P n ∣ n x x ] − 1 ( x n − x ^ n ∣ n ) = x n T [ P n ∣ n x x ] − 1 x n − x n T [ P n ∣ n x x ] − 1 x ^ n ∣ n − x ^ n ∣ n T [ P n ∣ n x x ] − 1 x n + x ^ n ∣ n T [ P n ∣ n x x ] − 1 x ^ n ∣ n ( 58 ) d:=(x_{n}-\hat x_{n|n})^{T}[P_{n|n}^{xx}]^{-1}(x_{n}-\hat x_{n|n}) \\=x_{n}^{T}[P_{n|n}^{xx}]^{-1}x_{n}-x_{n}^{T}[P_{n|n}^{xx}]^{-1}\hat x_{n|n} \\ -\hat x_{n|n}^{T}[P_{n|n}^{xx}]^{-1} x_{n}+\hat x_{n|n}^{T}[P_{n|n}^{xx}]^{-1} \hat x_{n|n}(58) d:=(xn−x^n∣n)T[Pn∣nxx]−1(xn−x^n∣n)=xnT[Pn∣nxx]−1xn−xnT[Pn∣nxx]−1x^n∣n−x^n∣nT[Pn∣nxx]−1xn+x^n∣nT[Pn∣nxx]−1x^n∣n(58)
同样，式(53)的右式中，后验概率密度的解析表达式中的指数项的指数可以写为
g : = ( q n − q ^ n ∣ n − 1 ) T [ P n ∣ n − 1 q q ] − 1 ( q n − q ^ n ∣ n − 1 ) − ( z n − z ^ n ∣ n − 1 ) T [ P n ∣ n − 1 z z ] − 1 ( z n − z ^ n ∣ n − 1 ) ( 59 ) g:=(q_{n}-\hat q_{n|n-1})^{T}[P_{n|n-1}^{qq}]^{-1}(q_{n}-\hat q_{n|n-1}) \\-(z_{n}-\hat z_{n|n-1})^{T}[P_{n|n-1}^{zz}]^{-1}(z_{n}-\hat z_{n|n-1})(59) g:=(qn−q^n∣n−1)T[Pn∣n−1qq]−1(qn−q^n∣n−1)−(zn−z^n∣n−1)T[Pn∣n−1zz]−1(zn−z^n∣n−1)(59)
使用式(52),则上式可以写为
g = [ x n − x ^ n ∣ n − 1 z n − z ^ n ∣ n − 1 ] T [ P n ∣ n − 1 x x P n ∣ n − 1 x z P n ∣ n − 1 z x P n ∣ n − 1 z z ] − 1 [ x n − x ^ n ∣ n − 1 z n − z ^ n ∣ n − 1 ] − ( z n − z ^ n ∣ n − 1 ) T [ P n ∣ n − 1 z z ] − 1 ( z n − z ^ n ∣ n − 1 ) ( 60 ) g= \begin{bmatrix} x_{n}-\hat x_{n|n-1}\\ z_{n}-\hat z_{n|n-1} \end{bmatrix}^{T}\begin{bmatrix} P_{n|n-1}^{xx}&P_{n|n-1}^{xz}\\ P_{n|n-1}^{zx}&P_{n|n-1}^{zz} \end{bmatrix}^{-1}\begin{bmatrix} x_{n}-\hat x_{n|n-1}\\ z_{n}-\hat z_{n|n-1} \end{bmatrix}\\-(z_{n}-\hat z_{n|n-1})^{T}[P_{n|n-1}^{zz}]^{-1}(z_{n}-\hat z_{n|n-1})(60) g=[xn−x^n∣n−1zn−z^n∣n−1]T[Pn∣n−1xxPn∣n−1zxPn∣n−1xzPn∣n−1zz]−1[xn−x^n∣n−1zn−z^n∣n−1]−(zn−z^n∣n−1)T[Pn∣n−1zz]−1(zn−z^n∣n−1)(60)
定义
[ C 11 C 12 C 21 C 22 ] : = [ P n ∣ n − 1 x x P n ∣ n − 1 x z P n ∣ n − 1 z x P n ∣ n − 1 z z ] − 1 ( 61 ) \begin{bmatrix} C_{11}&C_{12}\\ C_{21}&C_{22} \end{bmatrix}:=\begin{bmatrix} P_{n|n-1}^{xx}&P_{n|n-1}^{xz}\\ P_{n|n-1}^{zx}&P_{n|n-1}^{zz} \end{bmatrix}^{-1}(61) [C11C21C12C22]:=[Pn∣n−1xxPn∣n−1zxPn∣n−1xzPn∣n−1zz]−1(61)
其中，
C 11 = [ P n ∣ n − 1 x x − P n ∣ n − 1 x z ( P n ∣ n − 1 z z ) − 1 P n ∣ n − 1 z x ] − 1 ( 62 ) C_{11}=[P_{n|n-1}^{xx}-P_{n|n-1}^{xz}(P_{n|n-1}^{zz})^{-1}P_{n|n-1}^{zx}]^{-1}(62) C11=[Pn∣n−1xx−Pn∣n−1xz(Pn∣n−1zz)−1Pn∣n−1zx]−1(62)
C 12 = − C 11 P n ∣ n − 1 x z ( P n ∣ n − 1 z z ) − 1 ( 63 ) C_{12}=-C_{11}P_{n|n-1}^{xz}(P_{n|n-1}^{zz})^{-1}(63) C12=−C11Pn∣n−1xz(Pn∣n−1zz)−1(63)
C 21 = − C 22 P n ∣ n − 1 z x ( P n ∣ n − 1 x x ) − 1 ( 64 ) C_{21}=-C_{22}P_{n|n-1}^{zx}(P_{n|n-1}^{xx})^{-1}(64) C21=−C22Pn∣n−1zx(Pn∣n−1xx)−1(64)
C 22 = P n ∣ n − 1 z z − P n ∣ n − 1 z x ( P n ∣ n − 1 x x ) − 1 P n ∣ n − 1 x z ( 65 ) C_{22}=P_{n|n-1}^{zz}-P^{zx}_{n|n-1}(P^{xx}_{n|n-1})^{-1}P_{n|n-1}^{xz}(65) C22=Pn∣n−1zz−Pn∣n−1zx(Pn∣n−1xx)−1Pn∣n−1xz(65)
将式(61)-式(65)带入式(60)，可得
g = x n T C 11 x n + x n T [ − C 11 x ^ n ∣ n − 1 + C 12 ( z n − z ^ n ∣ n − 1 ) ] + . . . ( 66 ) g=x^{T}_{n}C_{11}x_{n}+x_{n}^{T}[-C_{11}\hat x_{n|n-1}+C_{12}(z_{n}-\hat z_{n|n-1})]+...(66) g=xnTC11xn+xnT[−C11x^n∣n−1+C12(zn−z^n∣n−1)]+...(66)
由于式(66)与式(58)等价，比较两式的第一项，我们可以得到
P n ∣ n x x = P n ∣ n − 1 x x − P n ∣ n − 1 x z ( P n ∣ n − 1 z z ) − 1 P n ∣ n − 1 z x ( 67 ) P_{n|n}^{xx}=P_{n|n-1}^{xx}-P_{n|n-1}^{xz}(P_{n|n-1}^{zz})^{-1}P_{n|n-1}^{zx}(67) Pn∣nxx=Pn∣n−1xx−Pn∣n−1xz(Pn∣n−1zz)−1Pn∣n−1zx(67)
比较第二项，可以得到
[ P n ∣ n x x ] − 1 x ^ n ∣ n = C 11 x ^ n ∣ n − 1 + C 12 ( z n − z ^ n ∣ n − 1 ) = [ P n ∣ n − 1 x x − P n ∣ n − 1 x z ( P n ∣ n − 1 z z ) − 1 P n ∣ n − 1 z x ] − 1 x ^ n ∣ n − 1 + [ P n ∣ n − 1 x x − P n ∣ n − 1 x z ( P n ∣ n − 1 z z ) − 1 P n ∣ n − 1 z x ] − 1 P n ∣ n − 1 z x × ( P n ∣ n − 1 z z ) − 1 ( z n − z ^ n ∣ n − 1 ) = [ P n ∣ n x x ] − 1 x ^ n ∣ n − 1 + [ P n ∣ n x x ] − 1 P n ∣ n − 1 z x ( P n ∣ n − 1 z z ) − 1 ( z n − z ^ n ∣ n − 1 ) ( 68 ) [P_{n|n}^{xx}]^{-1}\hat x_{n|n}=C_{11}\hat x_{n|n-1}+C_{12}(z_{n}-\hat z_{n|n-1}) \\=[P_{n|n-1}^{xx}-P_{n|n-1}^{xz}(P_{n|n-1}^{zz})^{-1}P_{n|n-1}^{zx}]^{-1}\hat x_{n|n-1} \\+[P_{n|n-1}^{xx}-P_{n|n-1}^{xz}(P_{n|n-1}^{zz})^{-1}P_{n|n-1}^{zx}]^{-1}P_{n|n-1}^{zx} \\ \times(P_{n|n-1}^{zz})^{-1}(z_{n}-\hat z_{n|n-1}) \\=[P_{n|n}^{xx}]^{-1}\hat x_{n|n-1}\\+[P_{n|n}^{xx}]^{-1}P_{n|n-1}^{zx}(P_{n|n-1}^{zz})^{-1}(z_{n}-\hat z_{n|n-1})(68) [Pn∣nxx]−1x^n∣n=C11x^n∣n−1+C12(zn−z^n∣n−1)=[Pn∣n−1xx−Pn∣n−1xz(Pn∣n−1zz)−1Pn∣n−1zx]−1x^n∣n−1+[Pn∣n−1xx−Pn∣n−1xz(Pn∣n−1zz)−1Pn∣n−1zx]−1Pn∣n−1zx×(Pn∣n−1zz)−1(zn−z^n∣n−1)=[Pn∣nxx]−1x^n∣n−1+[Pn∣nxx]−1Pn∣n−1zx(Pn∣n−1zz)−1(zn−z^n∣n−1)(68)
进一步推导上式， P n ∣ n x x P_{n|n}^{xx} Pn∣nxx左乘上式，可以得到
x ^ n ∣ n = x ^ n ∣ n − 1 + P n ∣ n − 1 z x ( P n ∣ n − 1 z z ) − 1 ( z n − z ^ n ∣ n − 1 ) ( 69 ) \hat x_{n|n}=\hat x_{n|n-1}+P_{n|n-1}^{zx}(P_{n|n-1}^{zz})^{-1}(z_{n}-\hat z_{n|n-1})(69) x^n∣n=x^n∣n−1+Pn∣n−1zx(Pn∣n−1zz)−1(zn−z^n∣n−1)(69)
定义卡尔曼增益为
K n : = P n ∣ n − 1 z x ( P n ∣ n − 1 z z ) − 1 ( 70 ) K_{n}:=P_{n|n-1}^{zx}(P_{n|n-1}^{zz})^{-1}(70) Kn:=Pn∣n−1zx(Pn∣n−1zz)−1(70)
现在，我们通过式(69)和式(67)就得到了卡尔曼滤波器的更新公式！！
x n ∣ n = x ^ n ∣ n − 1 + K n ( z n − z ^ n ∣ n − 1 ) ( 71 ) x_{n|n}=\hat x_{n|n-1}+K_{n}(z_{n}-\hat z_{n|n-1})(71) xn∣n=x^n∣n−1+Kn(zn−z^n∣n−1)(71)
P n ∣ n x x = P n ∣ n − 1 x x − K n ( P n ∣ n − 1 z z ) K n T ( 72 ) P_{n|n}^{xx}=P_{n|n-1}^{xx}-K_{n}(P_{n|n-1}^{zz})K_{n}^{T}(72) Pn∣nxx=Pn∣n−1xx−Kn(Pn∣n−1zz)KnT(72)
注意，我们没有对观测方程和动态方程做出任何线性假设。总结来说，卡尔曼滤波器的更新公式，可以由高斯分布直接推导得到，而不需要做其他任何假设。
3.高斯密度下贝叶斯点预测的积分运算
在上一节我们在高斯假设下，重新推导了卡尔曼滤波器的更新公式。在本节，我们再在高斯假设下，进一步完善卡尔曼预测公式。
我们假设 v n v_{n} vn是独立于 x n x_{n} xn与 z n z_{n} zn的零均值高斯随机噪声，服从高斯分布
v n − 1 ∼ p ( x n ∣ x n − 1 ) = N ( x n − [ f n ( x n − 1 ) + u n ] ; 0 , Q ) = N ( v n − 1 ; 0 , Q ) ( 73 ) v_{n-1} \sim p(x_{n}|x_{n-1})=N(x_{n}-[f_{n}(x_{n-1})+u_{n}];0,Q)=N(v_{n-1};0,Q)(73) vn−1∼p(xn∣xn−1)=N(xn−[fn(xn−1)+un];0,Q)=N(vn−1;0,Q)(73)
类似的，我们假设在观测方程中， w n w_{n} wn同样是独立于 x n x_{n} xn与 z n z_{n} zn的零均值高斯随机噪声，服从高斯分布
w n ∼ p ( z n ∣ x n ) = N ( z n − h n ( x n ) ; 0 , R ) = N ( w n ; 0. R ) ( 74 ) w_{n} \sim p(z_{n}|x_{n})=N(z_{n}-h_{n}(x_{n});0,R)=N(w_{n};0.R)(74) wn∼p(zn∣xn)=N(zn−hn(xn);0,R)=N(wn;0.R)(74)
根据贝叶斯准则，我们知道
p ( x n ∣ z 1 : n − 1 ) ∝ p ( z n ∣ x n ) p ( x n ∣ x n − 1 ) ( 75 ) p(x_{n}|z_{1:n-1})\propto p(z_{n}|x_{n})p(x_{n}|x_{n-1})(75) p(xn∣z1:n−1)∝p(zn∣xn)p(xn∣xn−1)(75)
由于两个高斯分布的概率密度函数的乘积仍然是服从高斯分布的，因此， p ( x n ∣ z 1 : n − 1 ) p(x_{n}|z_{1:n-1}) p(xn∣z1:n−1)也是高斯的。假设所有的概率密度函数都是高斯分布的，则
p ( x n ∣ z 1 : n − 1 ) = N ( x n ; x ^ n ∣ n − 1 , P n ∣ n − 1 x x ) ( 76 ) p(x_{n}|z_{1:n-1})=N(x_{n};\hat x_{n|n-1},P_{n|n-1}^{xx})(76) p(xn∣z1:n−1)=N(xn;x^n∣n−1,Pn∣n−1xx)(76)
p ( x n − 1 ∣ z 1 : n − 1 ) = N ( x n − 1 ; x ^ n − 1 ∣ n − 1 , P n − 1 ∣ n − 1 x x ) ( 77 ) p(x_{n-1}|z_{1:n-1})=N(x_{n-1};\hat x_{n-1|n-1},P_{n-1|n-1}^{xx})(77) p(xn−1∣z1:n−1)=N(xn−1;x^n−1∣n−1,Pn−1∣n−1xx)(77)
将式(77)带入式(44)、式(45)，并将式(76)带入式(46)、式(47)、式(48)，得到
x ^ n ∣ n − 1 = ∫ f n − 1 ( x n − 1 ) N ( x n − 1 ; x ^ n − 1 ∣ n − 1 , P n − 1 ∣ n − 1 x x ) d x n − 1 + u ( n ) ( 78 ) \hat x_{n|n-1}=\int f_{n-1}(x_{n-1})N(x_{n-1};\hat x_{n-1|n-1},P_{n-1|n-1}^{xx})dx_{n-1}+u(n)(78) x^n∣n−1=∫fn−1(xn−1)N(xn−1;x^n−1∣n−1,Pn−1∣n−1xx)dxn−1+u(n)(78)
P n ∣ n − 1 x x = ∫ [ f n − 1 ( x n − 1 ) + u n − x ^ n ∣ n − 1 ] [ f n − 1 ( x n − 1 ) + u n − x ^ n ∣ n − 1 ] T × N ( x n − 1 ; x ^ n − 1 ∣ n − 1 , P n − 1 ∣ n − 1 x x ) d x n − 1 + Q ( 79 ) P^{xx}_{n|n-1}=\int \left [f_{n-1}(x_{n-1})+u_{n}-\hat x_{n|n-1}\right ]\left [f_{n-1}(x_{n-1})+u_{n}-\hat x_{n|n-1}\right ]^{T}\\ \times N(x_{n-1};\hat x_{n-1|n-1},P_{n-1|n-1}^{xx})dx_{n-1}\\ +Q(79) Pn∣n−1xx=∫[fn−1(xn−1)+un−x^n∣n−1][fn−1(xn−1)+un−x^n∣n−1]T×N(xn−1;x^n−1∣n−1,Pn−1∣n−1xx)dxn−1+Q(79)
z ^ n ∣ n − 1 = ∫ h n ( x n ) N ( x n ; x ^ n ∣ n − 1 , P n ∣ n − 1 x x ) d x n ( 80 ) \hat z_{n|n-1}=\int h_{n}(x_{n})N(x_{n};\hat x_{n|n-1},P_{n|n-1}^{xx})dx_{n}(80) z^n∣n−1=∫hn(xn)N(xn;x^n∣n−1,Pn∣n−1xx)dxn(80)
P n ∣ n − 1 z z = ∫ [ h n ( x n ) − z ^ n ∣ n − 1 ] [ h n ( x n ) − z ^ n ∣ n − 1 ] T × N ( x n ; x ^ n ∣ n − 1 , P n ∣ n − 1 x x ) d x n + R ( 81 ) P^{zz}_{n|n-1}=\int [h_{n}(x_{n})-\hat z_{n|n-1}][h_{n}(x_{n})-\hat z_{n|n-1}]^{T}\\ \times N(x_{n};\hat x_{n|n-1},P_{n|n-1}^{xx})dx_{n}+R(81) Pn∣n−1zz=∫[hn(xn)−z^n∣n−1][hn(xn)−z^n∣n−1]T×N(xn;x^n∣n−1,Pn∣n−1xx)dxn+R(81)
P n ∣ n − 1 x z = ∫ [ x n − x ^ n ∣ n − 1 ] [ h n ( x n ) − z ^ n ∣ n − 1 ] T × N ( x n ; x ^ n ∣ n − 1 , P n ∣ n − 1 x x ) d x n ( 82 ) P^{xz}_{n|n-1}=\int [x_{n}-\hat x_{n|n-1}][h_{n}(x_{n})-\hat z_{n|n-1}]^{T}\\ \times N(x_{n};\hat x_{n|n-1},P_{n|n-1}^{xx})dx_{n}(82) Pn∣n−1xz=∫[xn−x^n∣n−1][hn(xn)−z^n∣n−1]T×N(xn;x^n∣n−1,Pn∣n−1xx)dxn(82)
其中，
R : = ∫ w n w n T N ( w n ; 0 , R ) d x n ( 83 ) R:=\int w_{n}w_{n}^{T}N(w_{n};0,R)dx_{n}(83) R:=∫wnwnTN(wn;0,R)dxn(83)
Q : = ∫ v n − 1 v n − 1 T N ( v n − 1 ; 0 , Q ) d x n − 1 ( 84 ) Q:=\int v_{n-1}v_{n-1}^{T}N(v_{n-1};0,Q)dx_{n-1}(84) Q:=∫vn−1vn−1TN(vn−1;0,Q)dxn−1(84)
终于，我们得到了在高斯假设下的预测公式。并且，离 线性卡尔曼滤波器越来越近！！
总结回顾下，在本章，我们在第一章结果的基础上，重新推导了在概率密度函数服从高斯分布的前提下，预测与更新步骤中的方程。目前，我们还未得到最后可以在工程中实际可用的解析解。
三.线性卡尔曼滤波器
在这一章，我们将在前面几章的基础上，推导线性卡尔曼滤波器的预测与更新公式。
1.线性状态方程
回顾前面几章，我们为了最后得到解析解，已经做出了一系列假设。我们假设系统服从一阶马尔可夫过程；我们假设使用最小均方准则作为代价函数；我们假设概率密度函数服从高斯分布…现在，我们再做出假设：假设状态方程是线性的，则动态方程可以重写为
x n = F x n − 1 + u n + v n − 1 ( 85 ) x_{n}=Fx_{n-1}+u_{n}+v_{n-1}(85) xn=Fxn−1+un+vn−1(85)
其中， F F F是与时间有关的状态转移矩阵。将式(85)带入式(78)，得到
x ^ n ∣ n − 1 = F ∫ x n − 1 N ( x n − 1 ; x ^ n − 1 ∣ n − 1 , P n − 1 ∣ n − 1 x x ) d x n − 1 + u ( n ) = F x ^ n − 1 ∣ n − 1 + u n ( 86 ) \hat x_{n|n-1}=F\int x_{n-1}N(x_{n-1};\hat x_{n-1|n-1},P_{n-1|n-1}^{xx})dx_{n-1}+u(n) \\= F\hat x_{n-1|n-1}+u_{n}(86) x^n∣n−1=F∫xn−1N(xn−1;x^n−1∣n−1,Pn−1∣n−1xx)dxn−1+u(n)=Fx^n−1∣n−1+un(86)
我们再将式(85)带入式(79)，得到
P n ∣ n − 1 x x = ∫ F [ x n − 1 − x ^ n ∣ n − 1 ] [ x n − 1 − x ^ n ∣ n − 1 ] T F T × N ( x n − 1 ; x ^ n − 1 ∣ n − 1 , P n − 1 ∣ n − 1 x x ) d x n − 1 + Q = F { ∫ [ x n − 1 − x ^ n ∣ n − 1 ] [ x n − 1 − x ^ n ∣ n − 1 ] T × N ( x n − 1 ; x ^ n − 1 ∣ n − 1 , P n − 1 ∣ n − 1 x x ) d x n − 1 } F T + Q ( 87 ) P^{xx}_{n|n-1}=\int F\left [x_{n-1}-\hat x_{n|n-1}\right ]\left [x_{n-1}-\hat x_{n|n-1}\right ]^{T}F^{T}\\ \times N(x_{n-1};\hat x_{n-1|n-1},P_{n-1|n-1}^{xx})dx_{n-1} +Q\\ =F\left \{\int \left [x_{n-1}-\hat x_{n|n-1}\right ]\left [x_{n-1}-\hat x_{n|n-1}\right ]^{T}\\ \times N(x_{n-1};\hat x_{n-1|n-1},P_{n-1|n-1}^{xx})dx_{n-1}\right \} F^{T}+Q(87) Pn∣n−1xx=∫F[xn−1−x^n∣n−1][xn−1−x^n∣n−1]TFT×N(xn−1;x^n−1∣n−1,Pn−1∣n−1xx)dxn−1+Q=F{∫[xn−1−x^n∣n−1][xn−1−x^n∣n−1]T×N(xn−1;x^n−1∣n−1,Pn−1∣n−1xx)dxn−1}FT+Q(87)
由于，
P n − 1 ∣ n − 1 x x = ∫ [ x n − 1 − x ^ n ∣ n − 1 ] [ x n − 1 − x ^ n ∣ n − 1 ] T × N ( x n − 1 ; x ^ n − 1 ∣ n − 1 , P n − 1 ∣ n − 1 x x ) d x n − 1 ( 88 ) P^{xx}_{n-1|n-1}=\int [x_{n-1}-\hat x_{n|n-1}][x_{n-1}-\hat x_{n|n-1}]^{T}\\ \times N(x_{n-1};\hat x_{n-1|n-1},P_{n-1|n-1}^{xx})dx_{n-1}(88) Pn−1∣n−1xx=∫[xn−1−x^n∣n−1][xn−1−x^n∣n−1]T×N(xn−1;x^n−1∣n−1,Pn−1∣n−1xx)dxn−1(88)
因此，
P n ∣ n − 1 x x = F P n − 1 ∣ n − 1 x x F T + Q ( 89 ) P^{xx}_{n|n-1}=FP^{xx}_{n-1|n-1}F^{T}+Q(89) Pn∣n−1xx=FPn−1∣n−1xxFT+Q(89)
2.线性观测方程
假设观测方程也是线性的，因此
z n = H x n + w n ( 90 ) z_{n}=Hx_{n}+w_{n}(90) zn=Hxn+wn(90)
其中， H H H是与时间和 x n x_{n} xn无关的观测矩阵。
因此，我们可以写为
h ( x n ) = H x n ( 91 ) h(x_{n})=Hx_{n}(91) h(xn)=Hxn(91)
假设，观测噪声 w n w_{n} wn是零均值过程，则式(80),式(81)可以更新为
z ^ n ∣ n − 1 = H x ^ n ∣ n − 1 ( 92 ) \hat z_{n|n-1}=H\hat x_{n|n-1}(92) z^n∣n−1=Hx^n∣n−1(92)
P n ∣ n − 1 z z = H P n ∣ n − 1 x x H T + R ( 93 ) P_{n|n-1}^{zz}=HP_{n|n-1}^{xx}H^{T}+R(93) Pn∣n−1zz=HPn∣n−1xxHT+R(93)
最后，将式(90),式(91)带入式(82)，得到
P n ∣ n − 1 x z = ∫ [ x n − x ^ n ∣ n − 1 ] [ z n − z ^ n ∣ n − 1 ] T × N ( x n ; x ^ n ∣ n − 1 , P n ∣ n − 1 x x ) d x n = ∫ [ x n − x ^ n ∣ n − 1 ] [ H x n + w n − H x ^ n ∣ n − 1 ] T × N ( x n ; x ^ n ∣ n − 1 , P n ∣ n − 1 x x ) d x n = P n ∣ n − 1 x x H T ( 94 ) P^{xz}_{n|n-1}=\int [x_{n}-\hat x_{n|n-1}][z_{n}-\hat z_{n|n-1}]^{T}\\ \times N(x_{n};\hat x_{n|n-1},P_{n|n-1}^{xx})dx_{n} \\=\int [x_{n}-\hat x_{n|n-1}][Hx_{n}+w_{n}-H \hat x_{n|n-1}]^{T}\\ \times N(x_{n};\hat x_{n|n-1},P_{n|n-1}^{xx})dx_{n} \\=P_{n|n-1}^{xx}H^{T}(94) Pn∣n−1xz=∫[xn−x^n∣n−1][zn−z^n∣n−1]T×N(xn;x^n∣n−1,Pn∣n−1xx)dxn=∫[xn−x^n∣n−1][Hxn+wn−Hx^n∣n−1]T×N(xn;x^n∣n−1,Pn∣n−1xx)dxn=Pn∣n−1xxHT(94)
3.总结
在观测方程与状态方程都满足严格线性的假设时，我们终于得到了可以使用的线性卡尔曼滤波器公式！！
针对卡尔曼滤波器的预测更新方程,现在做一个归纳总结。
Step1.状态向量的预测
x ^ n ∣ n − 1 = F x ^ n − 1 ∣ n − 1 + u n ( 95 ) \hat x_{n|n-1}= F\hat x_{n-1|n-1}+u_{n}(95) x^n∣n−1=Fx^n−1∣n−1+un(95)
P n ∣ n − 1 x x = F P n − 1 ∣ n − 1 x x F T + Q ( 96 ) P^{xx}_{n|n-1}=FP^{xx}_{n-1|n-1}F^{T}+Q(96) Pn∣n−1xx=FPn−1∣n−1xxFT+Q(96)
Step2.观测向量的预测
z ^ n ∣ n − 1 = H x ^ n ∣ n − 1 ( 97 ) \hat z_{n|n-1}=H\hat x_{n|n-1}(97) z^n∣n−1=Hx^n∣n−1(97)
P n ∣ n − 1 z z = H P n ∣ n − 1 x x H T + R ( 98 ) P_{n|n-1}^{zz}=HP_{n|n-1}^{xx}H^{T}+R(98) Pn∣n−1zz=HPn∣n−1xxHT+R(98)
P n ∣ n − 1 x z = P n ∣ n − 1 x x H T ( 99 ) P^{xz}_{n|n-1}=P_{n|n-1}^{xx}H^{T}(99) Pn∣n−1xz=Pn∣n−1xxHT(99)
Step3.卡尔曼滤波器更新
K n : = P n ∣ n − 1 z x ( P n ∣ n − 1 z z ) − 1 ( 100 ) K_{n}:=P_{n|n-1}^{zx}(P_{n|n-1}^{zz})^{-1}(100) Kn:=Pn∣n−1zx(Pn∣n−1zz)−1(100)
x n ∣ n = x ^ n ∣ n − 1 + K n ( z n − z ^ n ∣ n − 1 ) ( 101 ) x_{n|n}=\hat x_{n|n-1}+K_{n}(z_{n}-\hat z_{n|n-1})(101) xn∣n=x^n∣n−1+Kn(zn−z^n∣n−1)(101)
P n ∣ n x x = P n ∣ n − 1 x x − K n ( P n ∣ n − 1 z z ) K n T ( 102 ) P_{n|n}^{xx}=P_{n|n-1}^{xx}-K_{n}(P_{n|n-1}^{zz})K_{n}^{T}(102) Pn∣nxx=Pn∣n−1xx−Kn(Pn∣n−1zz)KnT(102)
不知道有没有读者觉得上文中卡尔曼增益的推导公式有些晦涩难懂？别担心。在线性假设的前提下，我们用一种更加简单的方式推导一下卡尔曼增益公式。
我们已知状态向量的滤波公式为 x ^ n ∣ n = x ^ n ∣ n − 1 + K n ( z n − z ^ n ∣ n − 1 ) \hat x_{n|n}=\hat x_{n|n-1}+K_{n}(z_{n}-\hat z_{n|n-1}) x^n∣n=x^n∣n−1+Kn(zn−z^n∣n−1)。记滤波误差为 x ~ n ∣ n = x ^ n ∣ n − x n \tilde{x}_{n|n} = \hat{x}_{n|n} - x_{n} x~n∣n=x^n∣n−xn,其中 x n x_{n} xn为 n n n时刻的真值。将式(101)代入，可得
x ~ n ∣ n = x ^ n ∣ n − 1 + K n ( z n − z ^ n ∣ n − 1 ) − x n = x ^ n ∣ n − 1 + K n ( H x n + v n ) − K n H x ^ n ∣ n − 1 − x n = ( I − K n H ) x ^ n ∣ n − 1 − ( I − K n H ) x n + K n v n = ( I − K n H ) ( x ^ n ∣ n − 1 − x n ) + K n v n = ( I − K n H ) x ~ n ∣ n − 1 + K n v n \begin{aligned} \tilde{x}_{n|n} &= \hat x_{n|n-1}+K_{n}(z_{n}-\hat z_{n|n-1}) - x_{n}\\ &= \hat x_{n|n-1} + K_{n}(Hx_{n}+v_{n}) - K_{n}H\hat x_{n|n-1}-x_{n}\\ &= (I-K_{n}H) \hat x_{n|n-1}-(I-K_{n}H)x_{n} + K_{n}v_{n}\\ &= (I-K_{n}H)(\hat x_{n|n-1} - x_{n})+K_{n}v_{n}\\ &= (I-K_{n}H)\tilde x_{n|n-1} +K_{n}v_{n} \end{aligned} x~n∣n=x^n∣n−1+Kn(zn−z^n∣n−1)−xn=x^n∣n−1+Kn(Hxn+vn)−KnHx^n∣n−1−xn=(I−KnH)x^n∣n−1−(I−KnH)xn+Knvn=(I−KnH)(x^n∣n−1−xn)+Knvn=(I−KnH)x~n∣n−1+Knvn
其中， v n v_{n} vn为 n n n时刻的观测噪声， x ~ n ∣ n − 1 \tilde x_{n|n-1} x~n∣n−1为预测误差。
由 P n ∣ n P_{n|n} Pn∣n与 x ~ n ∣ n \tilde x_{n|n} x~n∣n的关系可知
P n ∣ n = E { x ~ n ∣ n x ~ n ∣ n T } = E { ( ( I − K n H ) x ~ n ∣ n − 1 + K n v n ) ( ( I − K n H ) x ~ n ∣ n − 1 + K n v ) T } = ( I − K n H ) P n ∣ n − 1 ( I − K n H ) T + K n R n K n T = P n ∣ n − 1 − K n H T P n ∣ n − 1 T − K n H P n ∣ n − 1 + K n H P n ∣ n − 1 H T K n T + K n R n K n T \begin{aligned} P_{n|n}&=E\left\{ \tilde{x}_{n|n}\tilde{x}_{n|n}^{T}\right\}\\ &=E\left\{((I-K_{n}H)\tilde x_{n|n-1} +K_{n}v_{n})((I-K_{n}H)\tilde x_{n|n-1} +K_{n}v)^{T}\right\}\\ &= (I-K_{n}H)P_{n|n-1}(I-K_{n}H)^{T}+K_{n}R_{n}K_{n}^{T}\\ &= P_{n|n-1} - K_{n}H^{T}P_{n|n-1}^{T} - K_{n}HP_{n|n-1}+K_{n}HP_{n|n-1}H^{T}K_{n}^{T}+K_{n}R_{n}K_{n}^{T} \end{aligned} Pn∣n=E{x~n∣nx~n∣nT}=E{((I−KnH)x~n∣n−1+Knvn)((I−KnH)x~n∣n−1+Knv)T}=(I−KnH)Pn∣n−1(I−KnH)T+KnRnKnT=Pn∣n−1−KnHTPn∣n−1T−KnHPn∣n−1+KnHPn∣n−1HTKnT+KnRnKnT
取上式的迹并求关于 K n K_{n} Kn的导数可得
∂ t r { P n ∣ n } ∂ K n T = − P n ∣ n − 1 H T − ( H P n ∣ n − 1 ) T + 2 K n H P n ∣ n − 1 H T + 2 K n R n \begin{aligned} \frac{\partial tr\left\{ P_{n|n}\right\}}{\partial K_{n}^{T}}=-P_{n|n-1}H^{T}-(HP_{n|n-1})^{T}+2K_{n}HP_{n|n-1}H^{T}+2K_{n}R_{n} \end{aligned} ∂KnT∂tr{Pn∣n}=−Pn∣n−1HT−(HPn∣n−1)T+2KnHPn∣n−1HT+2KnRn
令导数等于0，可得
− P n ∣ n − 1 H T − ( H P n ∣ n − 1 ) T + 2 K n H P n ∣ n − 1 H T + 2 K n R n = 0 K n = P n ∣ n − 1 H T ( H P n ∣ n − 1 H T + R n ) − 1 -P_{n|n-1}H^{T}-(HP_{n|n-1})^{T}+2K_{n}HP_{n|n-1}H^{T}+2K_{n}R_{n}=0\\ K_{n} = P_{n|n-1}H^{T}(HP_{n|n-1}H^{T}+R_{n})^{-1} −Pn∣n−1HT−(HPn∣n−1)T+2KnHPn∣n−1HT+2KnRn=0Kn=Pn∣n−1HT(HPn∣n−1HT+Rn)−1
上式与式(100)等价。
此外，我们还得到了滤波协方差矩阵的另外两种表达式
P n ∣ n x x = P n ∣ n − 1 x x − P n ∣ n − 1 x z ( P n ∣ n − 1 z z ) − 1 P n ∣ n − 1 z x P_{n|n}^{xx}=P_{n|n-1}^{xx}-P_{n|n-1}^{xz}(P_{n|n-1}^{zz})^{-1}P_{n|n-1}^{zx} Pn∣nxx=Pn∣n−1xx−Pn∣n−1xz(Pn∣n−1zz)−1Pn∣n−1zx
P n ∣ n x x = ( I − K n H ) P n ∣ n − 1 ( I − K n H ) T + K n R n K n T P_{n|n}^{xx}=(I-K_{n}H)P_{n|n-1}(I-K_{n}H)^{T}+K_{n}R_{n}K_{n}^{T} Pn∣nxx=(I−KnH)Pn∣n−1(I−KnH)T+KnRnKnT
尾声
在第一章，我们从贝叶斯公式入手，先推导了在贝叶斯框架下，针对点估计问题，最优估计值的表达形式，得到了最优估计值与后验概率密度的关系式。接着，进一步考虑更实际的场景。由于实际问题中，我们是针对一个连续的动态过程做估计。因此，为了简化问题，假设该过程为一阶马尔可夫过程，并以此假设为前提，引出了过程方程和观测方程，同时，得到了相邻时刻的后验概率密度之间的递推公式。并发现，该递推过程，可大致分为预测与更新两个步骤。最后，再次利用最优估计值是后验概率密度的一阶矩的结论，分别推导了在预测与更新两个步骤中，后验概率密度的一阶矩与二阶矩的公式。
在第二章，我们做出了进一步的假设。假设概率密度函数都是高斯的，并在此假设下，修正了之前在预测与更新步骤中，所涉及的核心公式。并发现，在高斯假设下求得的更新公式与在最小均方准则和一阶马尔可夫过程假设下求得的更新公式是一样的。
在第三章，我们又一次做出了假设。假设动态方程与观测方程都是严格线性的，在多重假设下，我们进一步修正第二章中的预测步与更新步的核心公式，终于得到了大家最熟悉的卡尔曼滤波器。
不知有没有帮到各位读者？总之，完结撒花~