【多源融合】Sage-Husa滤波完整推导

不知道有没有同学跟我一样，有时候碰到公式推不出来强迫症就犯了，Sage-husa滤波虽然存在一些问题，但是其背后数学理论的推导还是比较难的，我也是到处看资料寻摸了一天，终于弄会了。

Sage滤波包括对R矩阵和Q矩阵的自适应，毕竟有时候观测环境和传感器数据特征不会完全符合经验模型，所以采用自适应的方法自动估计R和Q矩阵。本文接下来将主要推导三种Sage滤波：基于新息的观测噪声协方差矩阵（Innovation Adaptive Estimation, IAE）、基于残差的观测噪声协方差矩阵（Residual Adaptive Estimation, RAE）、系统状态噪声协方差矩阵。

基础知识

在$t_k$时刻先验状态$x_k$，先验状态协方差矩阵$P_k$，观测值$z_k$，后验状态${\hat{x}}_k$，后验状态协方差矩阵${\hat{P}}_k$。
动力学模型
$$x_k=A_k{\hat{x}}_{k-1}+w_k\text{\hspace{1em}(1)}$$
量测模型
$$z_k=H_k{x}_k+v_k\text{\hspace{1em}(2)}$$
其中$A_k$和$H_k$分别为状态转移矩阵和系数矩阵，$w_k$和$v_k$则是过程噪声和量测噪声，相应的协方差矩阵即为$Q_k$和$R_k$。

经典卡尔曼滤波状态更新过程
$$x_k=A_k{\hat{x}}_{k-1}\text{\hspace{1em}(3)}$$
$$P_k=A_k{\hat{P}}_{k-1}{A}_k^T+Q_k\text{\hspace{1em}(4)}$$
经典卡尔曼滤波量测更新过程
$$K_k=P_k{H}_k^T(H_k{P}_k{H}_k+R_k{)}^{-1}\text{\hspace{1em}(5)}$$
$${\hat{x}}_k=x_k+K_k(z_k-H_k{x}_k)\text{\hspace{1em}(6)}$$
$${\hat{P}}_k=(I-K_k{H}_k)P_k\text{\hspace{1em}(7)}$$
其中，I为单位阵，得到的$v_k=z_k-H_k{x}_k$称为新息向量，${\hat{v}}_k=z_k-H_k{\hat{x}}_k$称为残差向量，主要区别在于残差向量经过了观测值的校正，相比于新息向量，残差向量更能反应动力学模型的误差。

接下来介绍Sage滤波如何自适应求解R和Q。

最大似然估计与自适应滤波

Sage滤波思路，开窗平均代替真实随机模型
$$C_{v_k}=\frac{1}{N}\sum _{j=0}^N{v}_{k-j}{v}_{k-j}^T\text{\hspace{1em}(8)}$$
这里的$v$是误差，可以是新息向量，可以是残差向量，也可以是状态误差，相应协方差矩阵也是$R_k$或$Q_k$

在自适应因子$\alpha$为条件，观测值的概率密度函数写为
$$P\left(z_k|{\alpha }_k\right)=\frac{1}{\sqrt{(2\pi {)}^m\left|C_{v_k}\right|}}{e}^{-\frac{1}{2}{v}_k^T{C}_{v_k}^{-1}{v}_k}\text{\hspace{1em}(9)}$$
两边取对数
$$lnP\left(z_k|{\alpha }_k\right)=-\frac{1}{2}\left\{m\ast ln(2\pi )+ln(|C_{v_k}|)+v_k^T{C}_{v_k}^{-1}{v}_k\right\}\text{\hspace{1em}(10)}$$
那么式（10）中的最大似然也就变成了式（11）最小
$$\sum _{j=j_0}^{k}ln|C_{v_j}|+\sum _{j=j_0}^k{v}_j^T{C}_{v_j}^{-1}{v}_j=min\text{\hspace{1em}(11)}$$
进一步，对式（11）关于自适应因子$\alpha$求导
∑ j = j 0 k [ t r { C v j − 1 ∂ C v j ∂ α } − v j T C v j − 1 ∂ C v j ∂ α C v j − 1 v j ] = 0 (12) \sum_{j=j_0}^{k}[tr\{C_{v_j}^{-1}\frac {\partial C_{v_j}}{\partial \alpha} \}-v_j^TC_{v_j}^{-1}\frac {\partial C_{v_j}}{\partial \alpha}C_{v_j}^{-1}v_j]=0 \tag{12} j=j0​∑k​[tr{Cvj​−1​∂α∂Cvj​​​}−vjT​Cvj​−1​∂α∂Cvj​​​Cvj​−1​vj​]=0(12)

式（11）中出现了$C_{v_k}$，但是我们关心的是$R_k$和$Q_k$，将三者关系表达出来（$Q_k$包含在$P_k$里）
$$C_{v_k}=R_k+H_k{P}_k{H}_k^T\text{\hspace{1em}(13)}$$

对公式（13）关于自适应因子$\alpha$求导
$$\frac{\partial C_{v_k}}{\partial \alpha }=\frac{\partial R_k}{\partial \alpha }+H_k\frac{\partial P_k}{\partial \alpha }{H}_k^T\text{\hspace{1em}(14)}$$
将公式（4）带入式（14），并考虑稳态$\frac{\partial {\hat{P}}_{k-1}}{\partial \alpha }=0$有
$$\frac{\partial C_{v_k}}{\partial \alpha }=\frac{\partial R_k}{\partial \alpha }+H_k\frac{\partial Q_k}{\partial \alpha }{H}_k^T\text{\hspace{1em}(15)}$$
将公式（15）带入公式（12）
$$\sum _{j=j_0}^{k}tr\{[C_{v_j}^{-1}-C_{v_j}^{-1}{v}_j{v}_j^T{C}_{v_j}^{-1}][\frac{\partial R_j}{\partial \alpha }+H_j\frac{\partial Q_{j-1}}{\partial \alpha }{H}_j^T]\}=0\text{\hspace{1em}(16)}$$
公式（16）显示，$R_k$和$Q_k$均关于$\alpha$自适应。

基于新息的观测噪声协方差矩阵（IAE）

得到$R_k$自适应后，由开窗法估计新息协方差矩阵时，假设矩阵$Q_k$已知并且与$\alpha$无关，则有
$$\sum _{j=j_0}^{k}tr\{[C_{v_j}^{-1}-C_{v_j}^{-1}{v}_j{v}_j^T{C}_{v_j}^{-1}][I+0]\}=0\text{\hspace{1em}(17)}$$
开窗法估计新息协方差矩阵$C_{v_k}$
$$C_{v_k}=\frac{1}{N}\sum _{j=0}^N{v}_{k-j}{v}_{k-j}^T\text{\hspace{1em}(18)}$$
由新息向量$v_k=z_k-H_k{x}_k$和协方差传播律得
$$C_{v_k}=R_k+H_k{P}_k{H}_k^T\text{\hspace{1em}(19)}$$
进而可以估计$R_k$。（PS：这是最简单也最实用的）

基于残差的观测噪声协方差矩阵（RAE）

借助IAE中的思路，由残差向量${\hat{v}}_k$代替新息向量$v_k$求解协方差矩阵
$$C_{{\hat{v}}_k}=\frac{1}{N}\sum _{j=0}^N{\hat{v}}_{k-j}{\hat{v}}_{k-j}^T\text{\hspace{1em}(20)}$$
然后利用协方差传播律
$$C_{{\hat{v}}_k}=R_k+H_k{\hat{P}}_k{H}_k^T\text{\hspace{1em}(21)}$$
万事大吉！（做梦，这是完全错误的估计方法，应该是完事大吉）
要搞清楚，按照协方差传播律，谁才是求和结果，按照观测值和残差向量的定义，公式应为$z_k=H_k{\hat{x}}_k+{\hat{v}}_k$（新息向量由预测状态推导，残差向量则是由后验状态推导）
最终，真正的协方差传播律为
$$R_k=C_{{\hat{v}}_k}+H_k{\hat{P}}_k{H}_k^T\text{\hspace{1em}(22)}$$
但是式（11）中，要想估计$R_k$就要先知道${\hat{v}}_k$，但是这玩意儿又要求$R_k$先知道（死循环），所以采用前$N$个历元来近似估计当前时刻的$R_k$
$$R_k=\frac{1}{N}\sum _{j=1}^{N+1}{\hat{v}}_{k-j}{\hat{v}}_{k-j}^T+H_{k-1}{\hat{P}}_{k-1}{H}_{k-1}^T\text{\hspace{1em}(22)}$$
这是最简单的计算方法，如果对推导过程不关心的话，可以直接用上述的（22）就可以了。

接下来，详细推导公式（22）如何得到
由公式（17）得到，
∑ j = j 0 k t r { C v j − 1 − C v j − 1 v j v j T C v j − 1 } = 0 (23) \sum_{j=j_0}^{k}tr\{ C_{v_j}^{-1}-C_{v_j}^{-1}v_jv_j^TC_{v_j}^{-1} \}=0 \tag{23} j=j0​∑k​tr{Cvj​−1​−Cvj​−1​vj​vjT​Cvj​−1​}=0(23)
由卡尔曼滤波可以得到
$$R_k^{-1}{v}_k=C_{v_k}^{-1}{v}_k\text{\hspace{1em}(24)}$$
$$P_k{H}_k^T{C}_{v_k}^{-1}={\hat{P}}_k{H}_k^T{R}_k^{-1}\text{\hspace{1em}(25)}$$
公式（24）和（25）的推导参见附录A和附录B
将式（24）带入（23）得到
∑ j = j 0 k t r { R j − 1 [ R j C v j − 1 R j − v j v j T ] R j − 1 } = 0 (26) \sum_{j=j_0}^{k}tr\{ R_j^{-1}[R_jC_{v_j}^{-1}R_j-v_jv_j^T]R_j^{-1} \}=0 \tag{26} j=j0​∑k​tr{Rj−1​[Rj​Cvj​−1​Rj​−vj​vjT​]Rj−1​}=0(26)
式（25）左乘$H_k$得到
$$(C_{v_k}-R_k)C_{v_k}^{-1}=H_k{\hat{P}}_k{H}_k^T{R}_k^{-1}\text{\hspace{1em}(27)}$$
式（27）右乘$R_k$
$$R_k{C}_{v_k}^{-1}{R}_k=R_k-H_k{\hat{P}}_k{H}_k^T\text{\hspace{1em}(28)}$$
将式（28）带入到（26）中
∑ j = j 0 k t r { R j − 1 [ R j − H j P ^ j H j − v j v j T ] R j − 1 } = 0 (29) \sum_{j=j_0}^{k}tr\{ R_j^{-1}[R_j-H_j\hat P_jH_j-v_jv_j^T]R_j^{-1} \}=0 \tag{29} j=j0​∑k​tr{Rj−1​[Rj​−Hj​P^j​Hj​−vj​vjT​]Rj−1​}=0(29)
则式（29）的解为中括号“[]”内等于0，得到
$$R_k=C_{v_k}+H_k{\hat{P}}_k{H}_k\text{\hspace{1em}(30)}$$
即式（22）。

系统状态噪声协方差矩阵

假设此时$R_k$已知，且与$\alpha$无关，由式（16）得到
∑ j = j 0 k t r { H j T [ C v j − 1 − C v j − 1 v j v j T C v j − 1 ] H j } = 0 (31) \sum_{j=j_0}^{k}tr\{ H_j^T[C_{v_j}^{-1}-C_{v_j}^{-1}v_jv_j^TC_{v_j}^{-1}]H_j \}=0 \tag{31} j=j0​∑k​tr{HjT​[Cvj​−1​−Cvj​−1​vj​vjT​Cvj​−1​]Hj​}=0(31)
上述变换依据$tr(ABCD)=tr(BCDA)=tr(CDAB)=tr(DABC)$。
由卡尔曼滤波增益矩阵可以得到
$$H_k^T{C}_{v_k}^{-1}=P_k^{-1}{K}_k\text{\hspace{1em}(32)}$$
式（32）进行转置
$$C_{v_k}^{-1}{H}_k^T=K_k^T{P}_k^{-1}\text{\hspace{1em}(33)}$$
由于$C_{v_k}$和$P_k$均为对称矩阵，他们的逆等于矩阵本身。
将式（31）展开并带入式（32）和（33）
∑ j = j 0 k t r { H j T [ C v j − 1 − C v j − 1 v j v j T C v j − 1 ] H j } = ∑ j = j 0 k t r { H j T C v j − 1 H j − H j T C v j − 1 v j v j T C v j − 1 H j } = ∑ j = j 0 k t r { P j − 1 K j H j − P j − 1 K j v j v j T K j T P j − 1 } = ∑ j = j 0 k t r { P j − 1 ( K j H j P j − K j v j v j T K j T ) P j − 1 } − > ∑ j = j 0 k t r { K j H j P j − K j v j v j T K j T } = 0 (34) \begin{aligned} \sum_{j=j_0}^{k}tr\{ H_j^T[C_{v_j}^{-1}-C_{v_j}^{-1}v_jv_j^TC_{v_j}^{-1}]H_j \} &=\sum_{j=j_0}^{k}tr\{ H_j^TC_{v_j}^{-1}H_j-H_j^TC_{v_j}^{-1}v_jv_j^TC_{v_j}^{-1}H_j \} \\&=\sum_{j=j_0}^{k}tr\{ P_j^{-1}K_jH_j-P_j^{-1}K_jv_jv_j^TK_j^TP_{j}^{-1}\} \\&= \sum_{j=j_0}^{k}tr\{ P_j^{-1}(K_jH_jP_j-K_jv_jv_j^TK_j^T)P_{j}^{-1}\} \\&-> \sum_{j=j_0}^{k}tr\{ K_jH_jP_j-K_jv_jv_j^TK_j^T\} \\&=0 \end{aligned} \tag{34} j=j0​∑k​tr{HjT​[Cvj​−1​−Cvj​−1​vj​vjT​Cvj​−1​]Hj​}​=j=j0​∑k​tr{HjT​Cvj​−1​Hj​−HjT​Cvj​−1​vj​vjT​Cvj​−1​Hj​}=j=j0​∑k​tr{Pj−1​Kj​Hj​−Pj−1​Kj​vj​vjT​KjT​Pj−1​}=j=j0​∑k​tr{Pj−1​(Kj​Hj​Pj​−Kj​vj​vjT​KjT​)Pj−1​}−>j=j0​∑k​tr{Kj​Hj​Pj​−Kj​vj​vjT​KjT​}=0​(34)
在式（34）中，“->”代表化简，因为$P_k$中特征值均为正定，所以为了满足最后结果为0，对应矩阵运算的结果应该为0。
从卡尔曼滤波中可以推导出
$$\Delta x_k=K_k{v}_k\text{\hspace{1em}(35)}$$
$$K_k{H}_k{P}_k=P_k-{\hat{P}}_k\text{\hspace{1em}(36)}$$
将式（35）和（36）带入简化后的（34）中，
∑ j = j 0 k t r { P j − P ^ j − Δ x j Δ x j T } = 0 (37) \sum_{j=j_0}^{k}tr\{ P_j-\hat P_j-\Delta x_j\Delta x_j^T\}=0\tag{37} j=j0​∑k​tr{Pj​−P^j​−Δxj​ΔxjT​}=0(37)
将公式（4）带入（37）中并经过移项得到Sage自适应Q矩阵
$$Q_k=\frac{1}{N}\sum _{j=j_0}^k\Delta x_j\Delta x_j^T+{\hat{P}}_k-A_k{\hat{P}}_{k-1}{A}_k\text{\hspace{1em}(38)}$$

附录A

在附录A中，我们将证明如何得到公式（24）。
可以利用条件极值推导卡尔曼滤波解时，目标函数构建为
$$\Omega (k)=v_k^T{R}_k{v}_k+v_{x_k}^T{P}_k{v}_{x_k}-2{\lambda }^T(H_k{\hat{x}}_k-Z_k-v_k)=min\text{\hspace{1em}(A1)}$$
其中，$v_{x_k}={\hat{x}}_k-x_k$，$v_k=H_k{\hat{x}}_k-Z_k$，$\lambda$为拉格朗日乘数向量
对式（A1）分别对$v_k$和${\hat{x}}_k$求导
$$\frac{\partial \Omega (k)}{\partial v_k}=2v_k^T{R}_k+2{\lambda }^T=0\text{\hspace{1em}(A2)}$$
$$\frac{\partial \Omega (k)}{\partial {\hat{x}}_k}=2v_{x_k}^T{P}_k-2{\lambda }^T{H}_k=0\text{\hspace{1em}(A3)}$$
由式（A2）得到
$$v_k=-R_k^{-1}\lambda \text{\hspace{1em}(A4)}$$
由式（A3）推导
$$v_{x_k}=P_k^{-1}{H}_k^T\lambda \text{\hspace{1em}(A5)}$$
分别将式（A4）带入$v_k=H_k{\hat{x}}_k-z_k$，（A5）带入$v_{x_k}={\hat{x}}_k-x_k$得到
$$-R_k^{-1}\lambda =H_k{\hat{x}}_k-z_k\text{\hspace{1em}(A6)}$$
$$P_k^{-1}{H}_k^T\lambda ={\hat{x}}_k-x_k\text{\hspace{1em}(A7)}$$
式（A7）同时左乘矩阵$H_k$
$$H_k{P}_k^{-1}{H}_k^T\lambda =H_k{\hat{x}}_k-H_k{x}_k\text{\hspace{1em}(A8)}$$
式（A8）减去式（A6）得到
$$\lambda =(H_k{P}_k^{-1}{H}_k^T+R_k^{-1}{)}^{-1}(z_k-H_k{x}_k)\text{\hspace{1em}(A9)}$$
根据式（A4）和（A9）有
$$\lambda =-(C_{v_k})v_k=-R_k{v}_k\text{\hspace{1em}(A10)}$$
通过式（A10），（24）证毕。

附录B

在本节中，主要证明公式（25）。首先根据卡尔曼滤波公式（5）和（7），将（5）代入（7）
$${\hat{P}}_k=P_k-P_k{H}_k^T(H_k{P}_k{H}_k^T+R_k{)}^{-1}{H}_k{P}_k\text{\hspace{1em}(B1)}$$
由矩阵求逆引理
$$(A+BCD{)}^{-1}=A^{-1}-A^{-1}B(C^{-1}+D{A}^{-1}B{)}^{-1}D{A}^{-1}\text{\hspace{1em}(B2)}$$
将（B1）和（B2）对照可将式（B1）简化为
$${\hat{P}}_k=(P_k^{-1}+H_k^T{R}_k^{-1}{H}_k{)}^{-1}\text{\hspace{1em}(B3)}$$
根据公式（5）
$$\begin{array}{rl}{K}_k& =P_k{H}_k^T(H_k{P}_k{H}_k^T+R_k{)}^{-1}\\ & =[(H_k{P}_k{H}_k^T+R_k)(H_k^T{)}^{-1}{P}_k^{-1}{]}^{-1}\\ & =[H_k+R_k(H_k^T{)}^{-1}{P}_k^{-1}{]}^{-1}\\ & =[R_k(H_k^T{)}^{-1}(H_k^T{R}_k^{-1}{H}_k+P_k^{-1}){]}^{-1}\\ & =[H_k^T{R}_k^{-1}{H}_k+P_k^{-1}{]}^{-1}{H}_k^T{R}_k^{-1}\end{array}\text{\hspace{1em}(B4)}$$
将（B3）代入（B4）得
$$K_k={\hat{P}}_k{H}_k^T{R}_k^{-1}\text{\hspace{1em}(B5)}$$
此外，参考公式（5）和公式（19）
$$K_k=P_k{H}_k^T{C}_{v_k}^{-1}\text{\hspace{1em}(B6)}$$
通过式（B5）和（B6），公式（25）证毕。

参考文献

[1] 杨元喜. 自适应动态导航定位（第二版）.
[2] Mohamed A., Schwarz K. Adaptive Kalman Filtering for INS/GPS.
[3] 025卡尔曼滤波中滤波增益与协方差阵的等价形式. https://blog.csdn.net/Pro2015/article/details/82908148