卡尔曼滤波（Kalman Filtering）——（4）卡尔曼增益_综合

卡尔曼增益推导

一、状态空间方程
- 主要符号表
二、卡尔曼增益公式推导
三、Kalman滤波的几何解释
参考文献

一、状态空间方程

推导过程请看上一篇博客
状态方程：
$xk=Axk?1+Buk?1+wk?1(1)\begin{gathered} \boldsymbol{x}_{k}=\boldsymbol{A} \boldsymbol{x}_{k-1}+B u_{k-1}+\boldsymbol{w}_{k-1}\tag1 \end{gathered}$
观测方程：
$zk=Hxk+vk(2)\begin{gathered} \boldsymbol{z}_{k}=H \boldsymbol{x}_{k}+\boldsymbol{v}_{k}\tag2 \end{gathered}$
高斯噪音：
$p(ωk)?N(0,Q)p(vk)?N(0,R)\begin{aligned} &p\left(\omega_{k}\right) \sim N(0, Q) \\ &p\left(v_{k}\right) \sim N(0, R) \end{aligned}$
其中， 0 为期望值， $Q$ 为协方差矩阵。根据协方差定义 $Q=E(wwT)Q=E\left(\begin{array}{ll}ww^{T}\end{array}\right)$ , 可得:
$Q=E[[w1w2][w1w2]]=[Ew12E(w1w2)E(w2w1)Ew22]?已知Var(w)=E(w2)?(Ew)2又Ew=0?Var(w)=E(w2)=σw2=[σw12σw1σw2σw2σw1σw22]?负对角线σw1σw2不是两个标准差相乘，而是代表了w1和w2的协方差。\begin{aligned} Q &=E\left[\left[\begin{array}{l} w_{1} \\ w_{2} \end{array}\right]\left[\begin{array}{ll} w_{1} & w_{2} \end{array}\right]\right] \\ &=\left[\begin{array}{cc} E w_{1}^{2} & E (w_{1} w_{2}) \\ E (w_{2} w_{1}) & E w_{2}^{2} \end{array}\right] \\ &\longrightarrow 已知Var(w)=E(w^2)-(Ew)^2又Ew=0\Rightarrow Var(w)=E(w^2)= \sigma^2_w\\ &=\left[\begin{array}{cc} \sigma_{w_{1}}^{2} & \sigma_{w_{1}} \sigma_{w_{2}} \\ \sigma_{w_{2}} \sigma_{w_{1}} & \sigma_{w_{2}}^{2} \end{array}\right]\\ &\longrightarrow 负对角线\sigma_{w_{1}} \sigma_{w_{2}}不是两个标准差相乘，而是代表了w1和w2的协方差。 \end{aligned}$
为什么给系统加入的噪声必须是高斯白噪声？

高斯噪声在工程实践过程中非常常见；
卡尔曼滤波是针对线性系统而言的，而高斯噪声通过线性系统后其依旧服从高斯分布，对于后续比较好操作。

高斯噪声的线性变换

如上图所示，服从正态分布的噪声信号，通过线性系统变换后其结果仍然服从正态分布。

主要符号表

符号	意义	备注
$x_{k}$	系统状态矩阵(真实值)	计算得到
$z_{k}$	状态阵的观测量	实际测量
$A$	状态转移矩阵	–
$B$	控制输入矩阵	–
$H$	状态观测矩阵	–
$w_{k-1}$	过程噪声	服从正态分布
$v_{k}$	测量噪声	服从正态分布
$Q$	过程噪声协方差矩阵	–
$R$	测量噪声协方差矩阵	$E[v_{k} v_{k}^{T}]$
$x^k?\hat{\boldsymbol{x}}_{k}^{-}$	先验估计值	未处理的估计值
$x^kmea\hat{\boldsymbol{x}}_{k m e a}$	系统状态矩阵	实际测量
$x^k\hat{x}_{k}$	系统状态矩阵估计值	估计得到
$K_{k}$	卡尔曼增益	–
$e_{k}$	状态误差	$xk?x^kx_{k}-\hat{x}_{k}$
$P$	误差协方差矩阵	$E[e e^{T}]$
$e^-_{k}$	先验状态误差	$xk?x^k?x_{k}-\hat{x}_{k}^{-}$
$I$	单位阵	–
$P_{k}^{-}$	先验误差协方差矩阵	$E[e_{k}^{-} e_{k}^{-T}]$
$z_{k}$	状态阵的观测量	实际测量
$P_{k}$	后验误差协方差矩阵	$E[e_{k} e_{k}^{T}]$

二、卡尔曼增益公式推导

??前几篇博客都是对线性系统的卡尔曼进行了了解，而在生产生活中有太多的不确定性，对于大多数实际的控制系统而言，它并不是一个严格的线性时变系统(Linear Time System)，亦或系统结构参数的不确定性，导致估计的状态值存在偏差，而这个偏差值由过程噪声来表征。对于状态观测方程而言，我们知道采集的信号往往是包含噪声及干扰信号的，亦或观测矩阵自身的观测偏差，从而导致了测得的实际信号（观测值）与真实值之间存在一定的偏差。

不考虑噪声时，状态空间方程为：（一般情况下我们无法对噪声进行建模）
$x^k?=Ax^k?1+Buk?1(3)\hat{\boldsymbol{x}}_{k}^{-}=A \hat{\boldsymbol{x}}_{k-1}+B u_{k-1}\tag3$
事实上噪声是无法辟免的，因此公式 $(3)$ 求得的计算结果为未处理的估计值（未加入过程噪声只能是估计值），我们叫做先验估计值, 记作 $x^?\hat{\boldsymbol{x}}^{-}$ 。
对于测量值 $zk\boldsymbol{z}_{k}$ , 不考虑噪声时：
$zk=Hxk(4)\boldsymbol{z}_{k}=H \boldsymbol{x}_{k}\tag4$
根据公式 ( 4 ) 可求得：
$x^kmea=H?zk?H?为矩阵的逆(5)\hat{\boldsymbol{x}}_{k m e a}=H^{-} \boldsymbol{z}_{k}\tag5 \longrightarrow H^{-}为矩阵的逆$
因为公式 ( 3 ) ( 5 ) 均未考虑噪声，因此 $x^k?,x^kmea\hat{\boldsymbol{x}}_{k}^{-}, \hat{x}_{k m e a}$ 都是不准确的，都是缺少噪声的估计值。
前面博客已经提到：
$x^k=x^k?1+G(zk?x^k?1)(6)\hat{x}_{k}=\hat{x}_{k-1}+G\left(z_{k}-\hat{x}_{k-1}\right)\tag6$
在本例中 $zk=x^kmea,x^k?1=x^k?z_{k}=\hat{x}_{k m e a}, \hat{x}_{k-1}=\hat{x}_{k}^{-}$ , 将式 $(5)$ 代入式 $(6)$ 得：
$x^k=x^k?+G(H?zk?x^k?)(7)\hat{x}_{k}=\hat{x}_{k}^{-}+G\left(H^{-} z_{k}-\hat{x}_{k}^{-}\right)\tag7$
在教科书上一般表示 $G=K_{k} H$ , 代入式 $(7)$ 中得后验估计：
$x^k=x^k?+Kk(zk?Hx^k?)(8)\hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}\left(z_{k}-H \hat{x}_{k}^{-}\right)\tag8$
其中, $Kk∈[0,H?]K_{k} \in\left[0, H^{-}\right]$ , 当 $Kk=0,x^k=x^k?（算出来的结果）;K_{k}=0, \hat{x}_{k}=\hat{x}_{k}^{-}（算出来的结果） ;$ 当 $Kk=H?,x^k=H?zk0K_{k}=H^{-}, \hat{x}_{k}=H^{-} z_{k_{0}}$ （测出来的结果）
我们的目标是寻找 $K_{k}$ 使得 $x^k→xk,xk\hat{x}_{k} \rightarrow x_{k}, x_{k}$ 为真实值，即求与真实值最接近的估计值。真实情况下噪声是无法避免的，所以我们只能使 $x^k→xk,xk\hat{x}_{k} \rightarrow x_{k}, x_{k}$ 和 $x^k\hat{x}_{k}$ 之间存在误差，假设误差为
$ek=xk?x^k(9)e_{k}=x_{k}-\hat{x}_{k}\tag9$
误差服从正态分布，即 $p(ek)?(0,P),Pp\left(e_{k}\right) \sim(0, P), P$ 为协方差矩阵 :
$P=E[eeT]=[σe12σe1σe2σe2σe1σe22](10)P=E\left[\begin{array}{ll} e & e^{T} \end{array}\right]=\left[\begin{array}{cc} \sigma_{e _1}^{2} & \sigma_{e_{1}} \sigma_{e_{2}} \\ \sigma_{e_{2}} \sigma_{e_{1}} & \sigma_{e_{2}}^{2}\tag{10} \end{array}\right]$
根据方差的性质可知，方差越小，误差越小，估计值越接近真实值。我们希望选取合适的 $K_k$ ? 使得方差最小，即使得协方差矩阵 $P$ 的对角线之和——迹（Trace）的值最小。
$tr?(P)=σe12+σe22(11)\operatorname{tr}(P)=\sigma_{e_{1}}^{2}+\sigma_{e_{2}}^{2}\tag{11}$
为了求解 $K_{k}$ 使得 $tr?(P)\operatorname{tr}(P)$ 最小, 首先对 $P$ 进行详细推导：
$Pk=E[ekekT]=E[(xk?x^k)(xk?x^k)T](12)\begin{aligned} P_{k} &=E\left[\begin{array}{ll} e_k & e^{T}_k \end{array}\right] \\ &=E\left[\left(x_{k}-\hat{x}_{k}\right)\left(x_{k}-\hat{x}_{k}\right)^{T}\right]\tag{12} \end{aligned}$
在这里，我们应该先求出误差 $xk?x^kx_{k}-\hat{x}_{k}$ 的值, 将公式 $(8)$ 代入公式 $(9)$ 得 $:$
$xk?x^k=xk?(x^k?+Kk(zk?Hx^k?))=xk?x^k??Kkzk+KkHx^k?(13)\begin{aligned} x_{k}-\hat{x}_{k} &=x_{k}-\left(\hat{x}_{k}^{-}+K_{k}\left(z_{k}-H \hat{x}_{k}^{-}\right)\right) \\ &=x_{k}-\hat{x}_{k}^{-}-K_{k} z_{k}+K_{k} H \hat{x}_{k}^{-}\tag{13} \end{aligned}$
将 $z_{k}=H x_{k}+v_{k}$ 代入上式得：
$xk?x^k=xk?x^k??KkHxk?Kkvk+KkHx^k=(xk?x^k?)?KkH(xk?x^k?)?Kkvk=(I?KkH)(xk?x^k?)?Kkvk(14)\begin{aligned} x_{k}-\hat{x}_{k} &=x_{k}-\hat{x}_{k}^{-}-K_{k} H x_{k}-K_{k} v_{k}+K_{k} H \hat{x}_{k} \\ &=\left(x_{k}-\hat{x}_{k}^{-}\right)-K_{k} H\left(x_{k}-\hat{x}_{k}^{-}\right)-K_{k} v_{k} \\ &=\left(I-K_{k} H\right)\left(x_{k}-\hat{x}_{k}^{-}\right)-K_{k} v_{k}\tag{14} \end{aligned}$
其中，把 $xk?x^k?x_{k}-\hat{x}_{k}^{-}$ 叫做先验误差，记作 $e_{k}^{-}$ , 则上式为 :
$xk?x^k=(I?KkH)ek??Kkvk(15)x_{k}-\hat{x}_{k}=\left(I-K_{k} H\right) e_{k}^{-}-K_{k} v_{k}\tag{15}$
将式 $(15)$ 代入式 $(12)$ 中得：
$Pk=E[eeT]=E[(xk?x^k)(xk?x^k)T]=E[[(I?KkH)ek??Kkvk][(I?KkH)ek??Kkvk]T]=E[(I?KkH)ek?ek?T(I?KkH)T?(I?KkH)ek?vkTKkT?Kkvkek?T(I?KkH)T+KkvkvkTKkT]=E[(I?KkH)ek?ek?T(I?KkH)T]?E[(I?KkH)ek?vkTKkT]?E[Kkvkek?T(I?KkH)T]+E[KkvkvkTKkT](16)\begin{gathered} P_{k}=E\left[\begin{array}{cc} e & e^{T} \end{array}\right] \\ =E\left[\left(x_{k}-\hat{x}_{k}\right)\left(x_{k}-\hat{x}_{k}\right)^{T}\right] \\ =E\left[\left[\left(I-K_{k} H\right) e_{k}^{-}-K_{k} v_{k}\right]\left[\left(I-K_{k} H\right) e_{k}^{-}-K_{k} v_{k}\right]^{T}\right] \\ =E\left[\left(I-K_{k} H\right) e_{k}^{-} e_{k}^{-T}\left(I-K_{k} H\right)^{T}-\left(I-K_{k} H\right) e_{k}^{-} v_{k}^{T} K_{k}^{T}-K_{k} v_{k} e_{k}^{-T}\left(I-K_{k} H\right)^{T}+K_{k} v_{k} v_{k}^{T} K_{k}^{T}\right]\\ =E\left[\left(I-K_{k} H\right) e_{k}^{-} e_{k}^{-T}\left(I-K_{k} H\right)^{T}\right]-E\left[\left(I-K_{k} H\right) e_{k}^{-} v_{k}^{T} K_{k}^{T}\right] \\ -E\left[K_{k} v_{k} e_{k}^{-T}\left(I-K_{k} H\right)^{T}\right]+E\left[K_{k} v_{k} v_{k}^{T} K_{k}^{T}\right]\tag{16} \end{gathered}$
因为 $K_{k}, I-K_{k} H$ 为常数, 所以有：
$E[(I?KkH)ek?vkTKkT]=(I?KkH)E[ek?vkT]KkTE[Kkvkek?T(I?KkH)T]=KkE[vkek?T](I?KkH)T(17)\begin{aligned} E\left[\left(I-K_{k} H\right) e_{k}^{-} v_{k}^{T} K_{k}^{T}\right] &=\left(I-K_{k} H\right) E\left[e_{k}^{-} v_{k}^{T}\right] K_{k}^{T} \\ E\left[K_{k} v_{k} e_{k}^{-T}\left(I-K_{k} H\right)^{T}\right] &=K_{k} E\left[v_{k} e_{k}^{-T}\right]\left(I-K_{k} H\right)^{T}\tag{17} \end{aligned}$
$e_{k}^{-}$ 为先验误差， $v_{k}$ 为测量误差，两者相互独立，所以:
$E[ek?vkT]=0E[vkek?T]=0\begin{aligned} &E\left[e_{k}^{-} v_{k}^{T}\right]=0 \\ &E\left[v_{k} e_{k}^{-T}\right]=0 \end{aligned}$
代入式 (17)，有：
$Pk=E[(I?KkH)ek?ek?T(I?KkH)T]?0?0+E[KkvkvkTKkT]=(I?KkH)E[ek?ek?T](I?KkH)T+KkE[vkvkT]KkT(18)\begin{aligned} P_{k} &=E\left[\left(I-K_{k} H\right) e_{k}^{-} e_{k}^{-T}\left(I-K_{k} H\right)^{T}\right]-0-0+E\left[K_{k} v_{k} v_{k}^{T} K_{k}^{T}\right] \\ &=\left(I-K_{k} H\right) E\left[e_{k}^{-} e_{k}^{-T}\right]\left(I-K_{k} H\right)^{T}+K_{k} E\left[v_{k} v_{k}^{T}\right] K_{k}^{T}\tag{18} \end{aligned}$
式中, $E[ek?ek?T]E\left[e_{k}^{-} e_{k}^{-T}\right]$ 为 $P_{k}$ 的先验部分, 记作 $P_{k}^{-}$ , 则：
$Pk=(I?KkH)Pk?(I?KkH)T+KkE[vkvkT]KkT=(Pk??KkHPk?)(I?HTKkT)+KkRKkT=Pk??KkHPk??Pk?HTKkT+KkHPk?HTKkT+KkRKkT(19)\begin{aligned} P_{k} &=\left(I-K_{k} H\right) P_{k}^{-}\left(I-K_{k} H\right)^{T}+K_{k} E\left[v_{k} v_{k}^{T}\right] K_{k}^{T}\\ &=\left(P_{k}^{-}-K_{k} H P_{k}^{-}\right)\left(I-H^{T} K_{k}^{T}\right)+K_{k} R K_{k}^{T}\\ &=P_{k}^{-}-K_{k} H P_{k}^{-}-P_{k}^{-} H^{T} K_{k}^{T}+K_{k} H P_{k}^{-} H^{T} K_{k}^{T}+K_{k} R K_{k}^{T}\tag{19} \end{aligned}$
我们的目标是使 $tr?(Pk)\operatorname{tr}\left(P_{k}\right)$ 最小，下面我们对 $P_{k}$ 的各部分分别进行讨论。
$((Pk?HT)KkT)T=Kk(Pk?HT)T=KkHPk?T(20)\left(\left(P_{k}^{-} H^{T}\right) K_{k}^{T}\right)^{T}=K_{k}\left(P_{k}^{-} H^{T}\right)^{T}=K_{k} H P_{k}^{-T}\tag{20}$
转置后对角线上的值不变, 因此可得：
$tr?(KkHPk?)=tr?((Pk?HTKkT)T)=tr?(Pk?HTKkT)(21)\operatorname{tr}\left(K_{k} H P_{k}^{-}\right)=\operatorname{tr}\left(\left(P_{k}^{-} H^{T} K_{k}^{T}\right)^{T}\right)=\operatorname{tr}\left(P_{k}^{-} H^{T} K_{k}^{T}\right)\tag{21}$
对 $P_{k}$ 求迹等于对 $P_{k}$ 的各部分分别求迹, 即 :
$tr?(Pk)=tr?(Pk?)?2tr?(KkHPk?)+tr?(KkHPk?HTKkT)+tr?(KkRKkT)(22)\operatorname{tr}\left(P_{k}\right)=\operatorname{tr}\left(P_{k}^{-}\right)-2 \operatorname{tr}\left(K_{k} H P_{k}^{-}\right)+\operatorname{tr}\left(K_{k} H P_{k}^{-} H^{T} K_{k}^{T}\right)+\operatorname{tr}\left(K_{k} R K_{k}^{T}\right)\tag{22}$
再次虽调我们的目的是求得 $K_{k}$ 使得 $tr?(Pk)\operatorname{tr}\left(P_{k}\right)$ 最小, 所以将 $tr?(Pk)\operatorname{tr}\left(P_{k}\right)$ 对 $K_{k}$ 求导, 且导数为 0 时， $tr?(Pk)\operatorname{tr}\left(P_{k}\right)$ 求得极小值。已知 $dtr(AB)dA=BT,dtr(ABAT)dA=A（B+BT）=2AB(\frac{dt r(A B)}{d A}=B^{T}, \frac{d t r\left(A B A^{T}\right)}{d A}=A（B+B^T）=2 A B($ 推导过程省略 $)$ , 且 $tr?(Pk?)\operatorname{tr}\left(P_{k}^{-}\right)$ 与 $K_{k}$ 无关，有：
$dtr?(Pk)dKk=0?2(HPk?)T+2KkHPk?HT+2KkR=0??Pk?THT+KkHPk?HT+KkR=0??Pk?THT+Kk(HPk?HT+R)=0?Pk?T=Pk??Kk(HPk?HT+R)=Pk?THT?Kk=Pk?THTHPk?HT+R(23)\begin{aligned} \frac{d \operatorname{tr}\left(P_{k}\right)}{d K_{k}} &=0-2\left(H P_{k}^{-}\right)^{T}+2 K_{k} H P_{k}^{-} H^{T}+2 K_{k} R=0 \\ & \Rightarrow-P_{k}^{-T} H^{T}+K_{k} H P_{k}^{-} H^{T}+K_{k} R=0 \\ & \Rightarrow-P_{k}^{-T} H^{T}+K_{k}\left(H P_{k}^{-} H^{T}+R\right)=0 \longrightarrow P_{k}^{-T}=P_{k}^{-}\\ & \Rightarrow K_{k}\left(H P_{k}^{-} H^{T}+R\right)=P_{k}^{-T} H^{T} \\ & \Rightarrow K_{k}=\frac{P_{k}^{-T} H^{T}}{H P_{k}^{-} H^{T}+R} \\ \tag{23} \end{aligned}$
其中, $Kk∈[0,H?]K_{k} \in\left[0, H^{-}\right]$ , 当 $R$ 值很大（测量噪声很大）时， $Kk=0,x^k=x^k?K_{k}=0, \hat{x}_{k}=\hat{x}_{k}^{-}$ （算出来的结果） ; 当 $R$ 值很小（测量噪声很小）时， $Kk=H?,x^k=H?zk0K_{k}=H^{-}, \hat{x}_{k}=H^{-} z_{k_{0}}$ （测出来的结果）。

三、Kalman滤波的几何解释

??Kalman滤波公式可以用几何方式进行形象化的描述，这有助于增强对Kalman滤波含义的直观理解。
??如下图所示，已知上一时刻的状态估计 $x^k\hat{x}_{k}$ 和当前时刻的量测 $z_k$ ，若两者之间不共线,则可以共同确定一个平面，记为 $oαβo\alpha \beta$ ，在此平面基础上建立 $oαβγo\alpha \beta\gamma$ 空间直角坐标系。 $x^k\hat{x}_{k}$ 和 $z_k$ 经伸缩(线性变换)之后分别变成 $Gkx^kG_k\hat{x}_{k}$ 和 $K_kz_k$ 再合成为当前时刻的状态估计义，所以 $x^k\hat{x}_{k}$ 是 $x^k?1\hat{x}_{k-1}$ 和 $z_k$ 的线性组合， $x^k\hat{x}_{k}$ 也必定在 $oαβo\alpha \beta$ 平面内。但是,当前时刻的状态真值 $x_{k}$ 不一定恰好在 $oαβo\alpha \beta$ 平面内，只有当估计 $x^k\hat{x}_{k}$ 等于真值 $x_{k}$ 在 $oαβo\alpha \beta$ 平面上的正交投影时,估计误差 $ek=xk?x^ke_k=x_{k}-\hat{x}_{k}$ 才会最小。实现这一过程的关键在于确定系数矩阵 $K_k$ 。根据以上描述容易看出,估计误差 $x^k\hat{x}_{k}$ 必定同时垂直(不相关)于 $x^k?1\hat{x}_{k-1}$ 和 $z_k$ ，使用统计公式表示如下:
$E[ekzkT]=0E[ekxk?1T]=0\begin{aligned} E\left [ e_k z_k^\mathsf{T} \right ] =0\\ E\left [ e_k x_{k-1}^\mathsf{T} \right ] =0 \end{aligned}$

Kalman滤波的几何图示

??实际上，上述几何方式描述的正是Kalman滤波(线性最小方差无偏估计)的正交投影性质，即状态 $x_{k}$ 的最优估计 $x^k\hat{x}_{k}$ 是 $x_{k}$ 在由 $x^k?1\hat{x}_{k-1}$ 和 $Z_k$ 构成的线性空间上的正交投影。

参考文献

视频链接
【官方教程】卡尔曼滤波器教程与MATLAB仿真（全)(中英字幕)