【多源融合】自适应卡尔曼滤波的多种形式：遗忘卡尔曼滤波、渐消记忆卡尔曼滤波和自适应卡尔曼滤波

自适应卡尔曼滤波的多种形式

在卡尔曼滤波量测更新过程中，按照平差中的数学模型分为函数模型和随机模型，那么动力学方法和观测方程均为函数模型，P和R矩阵的确定则为随机模型的建立。

对于P矩阵来说，其核心思想在于重用当前时刻观测值。在融合初始化过程中，我们会对P矩阵进行初始化，之后随状态更新和量测更新而更新，一旦预测状态或者观测值突然出现问题，经验随机模型无法描述这种误差。

因此，研究者们提出了自适应滤波，使得滤波更相信当前时刻的观测（这也是合理的，对于观测是否合理则采用抗差滤波的方式），进而补偿P这部分随机模型误差。随之，遗忘滤波、渐消记忆滤波和自适应滤波逐渐发展，但是这些滤波容易混淆。本文将对四种形式的自适应滤波进行系统的总结，按照自适应因子的构建分为遗忘滤波、渐消记忆滤波、自适应滤波、sage-husa自适应滤波。前三种都需要构建自适应因子（遗忘因子、渐消记忆因子和自适应因子），最后一种则采用开窗平均的方法代替自适应因子的构建。

注意：其实遗忘因子是渐消记忆因子的另一种叫法，但是本文根据不同模型将遗忘因子和渐消记忆因子进行了区分，可以看成把简单问题复杂化了吧，而且遗忘因子/渐消记忆因子的求解也不止这两种方法。

经典卡尔曼滤波

遗忘滤波

渐消记忆滤波

自适应滤波

Sage-Husa自适应滤波

1 经典卡尔曼滤波

推导过程不再细述，直接上公式

系统动力学方程和观测方程（平差中的函数模型）：
$$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)}$$

状态更新方程：
$$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^T+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)}$$

式中，$Q_k$和$R_k$分别是$w_k$和$v_k$的协方差矩阵，两种噪声互不相关。在每个历元进行状态更新，每个观测历元进行量测更新，形成时间上的迭代估计最优状态。

2 遗忘滤波

遗忘滤波相比经典卡尔曼滤波多了遗忘因子，记为$\lambda$，改进公式（4）为公式（8）
$$P_k=A_k\frac{1}{\lambda }{\hat{P}}_{k-1}{A}_k^T+Q_k\text{\hspace{1em}(8)}$$
其中遗忘因子$\lambda$为一常数，取值0.95-0.995。

遗忘因子通过修改先验协方差矩阵$P_k$，调节当前历元中预测和量测之间的权比关系。遗忘因子设置越大，则前一历元先验协方差矩阵$P_{k-1}$越接近传统卡尔曼滤波后验协方差矩阵${\hat{P}}_{k-1}$，预测和量测之间权比关系越接近没有改变，特殊情况$\lambda =1$；反之，遗忘因子设置的越小，那么当前历元先验协方差矩阵$P_k$就会越大，使得卡尔曼滤波增益矩阵$K_{k+1}$越大，越相信下当前时刻的量测。也就是说，新数据（观测值）的权重要大于历史数据（预测值）。
总结一下，遗忘因子的作用在于通过调节协方差矩阵，权衡预测状态和观测值的权重，遗忘因子一般大于1。

但是，遗忘因子存在一定的缺陷，因为采用的是常值遗忘因子，无法适应环境变化；其次，人为降低历史状态信息，在预测准确而量测不准确的情况下，得到效果不好。因此，有学者对探究对遗忘因子常值取指数的方法，${(\frac{1}{\lambda })}^{N-k}$，N是一个参考时刻，要大于k，k为当前历元数目，通过指数遗忘因子可以看出，距离N越远，则遗忘因子越大，预测越不可靠，但是理论推导也不是很严谨，也存在预测准确量测不准确时面临的问题。

3 渐消记忆滤波

渐消记忆滤波构造了渐消记忆因子，渐消记忆因子其实是遗忘因子的另一种叫法，渐消记忆因子也有很多求解方法，本质上也是对式（4）进行修改至式（9），这里记渐消记忆因子为$s$，$s\ge 1$。
$$P_k=A_{k}s{\hat{P}}_{k-1}{A}_k^T+Q_k\text{\hspace{1em}(9)}$$

夏启军学者在90年发表于自动化学报中的论文《渐消卡尔曼滤波的最佳自适应算及其应用》一直延续到现在，最常用的还是他第二类算法，最佳遗忘因子的一步算法，本文将介绍该算法模型。

渐消记忆因子求解
s = m a x { 1 , 1 n t r ( N k M k − 1 ) } (10) s=max\{1,\frac{1}{n}tr(N_kM_k^{-1})\}\tag{10} s=max{1,n1​tr(Nk​Mk−1​)}(10)
其中tr()是矩阵求迹的符号，$M_k$和$N_k$表达式为
$$M_k=H_k{A}_k{\hat{P}}_{k-1}{A}_k^T{H}_k^T\text{\hspace{1em}(11)}$$
$$N_k=P_{vk}-H_k{Q}_k{H}_k^T-R_k\text{\hspace{1em}(12)}$$
其中，$P_{vk}=E(V_k{V}_k^T)$，$V_k=A_k{x}_k-L_k$，$V_k$称为预测残差，一般通过开窗估计法确定$P_{vk}$
$${\tilde{P}}_{vk}=\frac{1}{k}\sum _{i=1}^k{V}_i{V}_i^T\text{\hspace{1em}(13)}$$

通过对式（11）进行分析$M_k=P_k=E(x_k{x}_k^T)=E(H_k{A}_k{\hat{x}}_{k-1}(H_k{A}_k{\hat{x}}_{k-1}{)}^T)$，同理，式（12）也代表先验状态$x_k$的协方差矩阵不过相比于$M_k$，$N_k$中包含了量测更新结果，即$N_k$和$M_k$迹的比值为先验状态协方差矩阵在量测和预测时的比值。

式（9）的简便估计算法
s = m a x { 1 , t r ( N k ) t r ( M k ) } (14) s=max\{1,\frac{tr(N_k)}{tr(M_k)}\}\tag{14} s=max{1,tr(Mk​)tr(Nk​)​}(14)

但是，式（12）中，由于开窗估计法难以解决环境突变问题，即观测突变，带来的随机模型误差，有的学者对此进行了改进
$${\tilde{P}}_{vk}=\{\begin{array}{rcl}\frac{s_{k-1}{V}_k{V}_k^T}{1+s_{k-1}}& & k>1\\ \frac{1}{2}{V}_0{V}_0^T& & k=1\end{array}\text{\hspace{1em}(15)}$$
通过对比式（13）和（15）经过修改后的自适应因子对本历元的环境突变更加敏感。

渐消记忆因子也是对随机模型的误差进行补偿，经过修改后，下一历元的先验协方差矩阵大于等于当前时刻传统卡尔曼滤波后验状态协方差矩阵（对比），进而调整预测值和观测值间的权重关系。

注意：如果按照严恭敏老师《捷联惯导算法与组合导航原理》P163中的说法，单重渐消记忆因子卡尔曼滤波又被称为强跟踪卡尔曼滤波。
按照字面意思理解，强跟踪滤波跟踪的是目标的运动状态，假设运动状态匀速开始，突然变为加速，此时先验残差增加，自适应因子增加，导致先验P矩阵增加。（感谢师弟的例子）

4 自适应滤波

自适应滤波的目标是通过自适应因子调节先验P矩阵，平衡历史状态和观测值的权。严格来说，遗忘因子、渐消记忆因子都是自适应因子的一种。具有和式（8）式（9）一样的公式调节形式，这里我们定义自适应因子为$\alpha$
$$P_k=A_k\alpha {\hat{P}}_{k-1}{A}_k^T+Q_k\text{\hspace{1em}(16)}$$

4.1 误差判别统计量

对比遗忘因子和渐消记忆因子来说，自适应因子具有更直观的理论依据，主要通过构建误差判别统计量估计自适应因子，详细内容可见杨院士论文《自适应抗差滤波理论及应用的主要进展》，误差判别统计量有状态不符值统计量、速度不符值统计量、预测残差统计量和方差分量比统计量。

4.1.1 状态不符值统计量

利用状态参数估计向量${\hat{x}}_k$（滤波解）和预测状态向量$x_k$构建统计量
$${\hat{x}}_k-x_k=(\Delta {\hat{x}}_1^2+\Delta {\hat{x}}_2^2+\dots +\Delta {\hat{x}}_k^2{)}^{\frac{1}{2}}\text{\hspace{1em}(17)}$$
判别统计量
$$\Delta {\hat{x}}_k=\frac{{\hat{x}}_k-x_k}{\sqrt{tr(P_k)}}\text{\hspace{1em}(18)}$$
简单分析：（1）计算历元的观测数量要大于待估状态参数的数量；（2）观测量估计的状态参数向量${\hat{x}}_k$应尽可能精确，否则统计量$\Delta {\hat{x}}_k$不能反映动力学模型的误差；（3）统计量$\Delta {\hat{x}}_k$反映模型的整体误差，任何状态分量的扰动都将视为整体模型存在扰动。

4.1.2 速度不符值统计量

根据状态得到相邻历元$k$和$k-1$时间内速度估计为
$$\dot{{\hat{x}}_k}=\frac{{\hat{x}}_k-{\hat{x}}_k}{t_k-t_{k-1}}\text{\hspace{1em}(19)}$$
相应的滤波速度与估计速度不符值函数为
$$\Delta \dot{{\hat{x}}_k}=\frac{\dot{{\hat{x}}_k}-\dot{x_k}}{\sqrt{tr(P_k)}}\text{\hspace{1em}(20)}$$
此时的$P_k$对应于$\dot{{\hat{x}}_k}$的协方差矩阵。
简单分析：（1）如果$\Delta \dot{{\hat{x}}_k}$显著异常，则表明预测速度存在异常，或动力学模型存在较大误差；（2）$\Delta \dot{{\hat{x}}_k}$的计算要求有多余观测信息。

4.1.3 预测残差统计量

$$\Delta V_k=(\frac{V_k^T{V}_k}{tr(P_{v_k})}{)}^{\frac{1}{2}}\text{\hspace{1em}(21)}$$
简单分析：（1）不需要在滤波前计算状态向量参考值；（2）不需要观测量个数一定大于状态参数个数；（3）与统计量$\Delta {\hat{x}}_k$相比，可能含有更多的测量误差（不只是观测误差）。

4.1.4 方差分量比统计量

如果将$t_k$时刻的观测值$L_k$和状态先验$x_k$看成两组观测向量，则他们的方差分量应能反映其相应的观测精度和模型精度（量测与预测），于是可用方差分量比构造误差判别统计量。其中，$L_k$和$x_k$的Helmert方差分量估计公式为
$${\hat{\sigma }}_{v_k}^2=\frac{V_k^T{P}_k{V}_k}{r_k}\text{\hspace{1em}(21)}$$
$${\hat{\sigma }}_{x_k}^2=\frac{V_{x_k}^T{P}_{x_k}{V}_{x_k}}{r_x}\text{\hspace{1em}(22)}$$
式（21）和（22）分别为观测值和先验值得方差分量，$r_k$和$r_x$分别为观测值和先验值冗余数目，$V_k$和$V_{x_k}$分别为k时刻观测值和先验值的残差向量。
$$V_{x_k}={\hat{x}}_k-x_k\text{\hspace{1em}(23)}$$
由方差分量比构造的模型误差统计量为
$$S_k=\frac{{\hat{\sigma }}_{x_k}^2}{{\hat{\sigma }}_{v_k}^2}\approx \frac{V_{x_k}^T{P}_{x_k}{V}_{x_k}}{r_x{\hat{\sigma }}_{v_k}^2}\text{\hspace{1em}(24)}$$
简单分析：（1）$S_k$的计算需要冗余观测，否则统计量不能有效地反映模型误差；（2）$V_k$和$V_{x_k}$对应相同的状态估计量${\hat{x}}_k$；（3）如果采用迭代计算$S_k$的计算量稍大于$\Delta {\hat{x}}_k$和$\Delta {\hat{V}}_k$。

4.2 自适应因子

自适应因子的构建以其分段数目分为了二段式和三段式，还有指数函数表示的自适应因子和选权法表示的自适应因子，主要使用形式为前两种。接下来，以统计量$\Delta x_k$为例进行说明。

4.2.1 三段式自适应因子

$${\alpha }_k=\{\begin{array}{rcl}1& & \Delta x_k\le c_0\\ \frac{c_0}{\Delta x_k}(\frac{c_1-|\Delta x_k|}{c_1-c_0}{)}^2& & k>1\\ 0& & \Delta x_k>c_1\end{array}\text{\hspace{1em}(25)}$$
其中，$c_0$和$c_1$均为常值，取值范围$c_0=1.0-1.5$，$c_1=3.0-8.0$。

4.2.2 二段式自适应因子

$${\alpha }_k=\{\begin{array}{rcl}1& & \Delta x_k\le c\\ \frac{c}{\Delta x_k}& & \Delta x_k>c\end{array}\text{\hspace{1em}(26)}$$
式中，$c_1$为常数，取值范围$c_1=1.0-2.5$。
相比于三段式自适应因子，二段式自适应因子永不为0，即历史状态信息影响最优状态估计结果，并且其减小趋势缓于三段式。

4.2.3 指数函数表示的自适应因子

$${\alpha }_k=\{\begin{array}{rcl}1& & \Delta x_k\le c\\ e^{-(\Delta x_k-c{)}^2}& & \Delta x_k>c\end{array}\text{\hspace{1em}(27)}$$
其中，c也为常数，与4.2.2中结构类似。

4.2.4 选权法表示的自适应因子

$${\alpha }_k=\{\begin{array}{rcl}1& & \Delta x_k\le c\\ 0& & \Delta x_k>c\end{array}\text{\hspace{1em}(26)}$$

5 Sage-Husa自适应滤波

Sage-Husa滤波方法采用开窗法，即滑动窗口，确定当前历元观测向量协方差矩阵$R_k$和模型误差协方差矩阵$Q_k$。相比于第4章中的自适应滤波，Sage滤波不需要已知各种方差等参数，但是需要提前确定窗口大小，窗口大小的选择影响到定位精度。窗口选择太大，遇到突变情况无法反映真实结果，窗口选择太小，又不足以刻画相应误差特征。

在开窗估计法之前先介绍一下残差向量和新息向量，即预测残差向量，分别为
$${\hat{V}}_k=z_k-H_k{\hat{x}}_k\text{\hspace{1em}(27)}$$
$$V_k=z_k-H_k{x}_k\text{\hspace{1em}(28)}$$
${\hat{V}}_k$经过观测信息修正，包含动力学模型误差和量测误差，$V_k$未经过观测值修正，称为新息，新息向量更能反应动态系统的扰动误差，二者之间的关系为
$${\hat{V}}_k=(I-H_k{K}_k)V_k\text{\hspace{1em}(29)}$$
${\hat{V}}_k$和$V_k$的协方差矩阵分别为
$${\hat{P}}_{{\hat{V}}_k}=R_k-H_k{\hat{P}}_k{H}_k^T\text{\hspace{1em}(30)}$$
$$P_{V_k}=R_k+H_k{P}_k{H}_k^T\text{\hspace{1em}(31)}$$
一开始，楼主通过式（27）和（28）利用协方差传播律去解释式（30）和（31），但是发现（31）符号有误。其实，二者函数式不一样。在新息向量中，新息由观测值和预测值组成，残差向量中，观测值由后验状态和残差组成，再按照协方差传播律去解释就行了。要搞清楚谁加谁，谁减谁。（不知道理解是否正确，欢迎指教）

5.1 观测噪声协方差矩阵的开窗估计

观测噪声协方差矩阵的开窗估计方法可以采用预测残差向量或观测残差向量进行估计，前者为先验状态估计的预测残差，后者为后验状态的验后残差。

第一种，预测残差向量开窗估计法。
$$P_{V_k}=\frac{1}{N}\sum _{j=0}^N{V}_{k-j}{V}_{k-j}^T\text{\hspace{1em}(32)}$$
则$t_k$时刻观测值协方差矩阵
$$R_k=P_{V_k}-H_k{P}_k{H}_k^T\text{\hspace{1em}(33)}$$

第二种，观测残差向量开窗估计法
$$P_{{\hat{V}}_k}=\frac{1}{N}\sum _{j=0}^N{\hat{V}}_{k-j}{\hat{V}}_{k-j}^T\text{\hspace{1em}(34)}$$
则$t_k$时刻观测值协方差矩阵
$$R_k=P_{{\hat{V}}_k}+H_k{\hat{P}}_k{H}_k^T\text{\hspace{1em}(35)}$$
然而，由式（35）估计$R_k$时，需要先得到${\hat{P}}_k$，二者相悖，因此可以利用前$N$个历元估计当前时刻的$R_k$，式（34）和（35）转化为（36）和（37）
$$P_{{\hat{V}}_k}=\frac{1}{N}\sum _{j=1}^{N+1}{\hat{V}}_{k-j}{\hat{V}}_{k-j}^T\text{\hspace{1em}(36)}$$
$$R_k=P_{{\hat{V}}_{k-1}}+H_{k-1}{\hat{P}}_{k-1}{H}_{k-1}^T\text{\hspace{1em}(37)}$$

注意：（1）第一种方法内包含历史状态中的误差，若$x_k$误差越大，则$V_k$误差越大，计算的$R_k$稳定性也就越差；
（2）第一种方法可能出现负定现象；
（3）两种方法都是对前N个历元观测信息进行了平均，要求当前时刻观测环境要与历史的N个观测环境一致，“这种自适应估计，很难真正实现‘自适应’”。
（4）两种方法都要求矩阵同类别、同分布、同维度。

5.2 动态模型噪声矩阵的开窗估计

令状态改正值$V_{x_k}={\hat{x}}_k-x_k$，容易得到
$$Q_k=P_{V_{x_k}}+{\hat{P}}_k-A_k{\hat{P}}_{k-1}{A}_k^T\text{\hspace{1em}(38)}$$
$V_{x_k}$协方差矩阵为（开窗估计）
$$P_{V_{x_k}}=\frac{1}{N}\sum _{j=0}^N{V}_{x_k}{V}_{x_k}^T\text{\hspace{1em}(39)}$$
然而，该方法求$V_{x_k}$要先求$Q_k$，为了避免这个问题，直接估计$Q_k$。
注意到$V_{x_k}$和$V_k$，
$$V_{x_k}=-K_k{V}_k\text{\hspace{1em}(40)}$$
进一步有
$$P_{V{x}_k}=K_k{P}_{V_k}{K}_k^T\text{\hspace{1em}(41)}$$
在由5.1节估计出$P_{V_k}$之后，在稳态情况下，可以直接用$P_{V_{x_k}}$近似代替$Q_k$。

注意：利用自适应估计时，状态噪声协方差矩阵和观测噪声协方差矩阵应当一方减小一方增加，若二者同时增加或减小可能会导致滤波发散。

参考文献

1、卡尔曼滤波器的遗忘因子. https://blog.csdn.net/qwe900/article/details/105867521.
2、信息滤波、UD滤波、遗忘滤波和自适应滤波. https://blog.csdn.net/Ruins_LEE/article/details/116769786.
3、夏启军,孙优贤,周春晖.渐消卡尔曼滤波器的最佳自适应算法及其应用[J].自动化学报,1990(03):210-216.
4、杨元喜. 自适应动态导航定位（第二版）。（强烈推荐）
5、严恭敏.捷联惯导算法与组合导航原理。
6、杨元喜，任夏，许艳. 自适应抗差滤波理论及应用的主要进展。