1 简介

传统的卡尔曼滤波算法假定了噪声服从高斯分布，而实际应用场景中，由于传感器受到各种因素的影响，可能存在部分远偏离预期值的观测结果，称为奇异值。此时观测噪声不再是高斯分布的，而类似于student-t分布，对于这样的情况，传统的卡尔曼滤波算法在估计系统状态时会极大地受到奇异值带来的影响，从而导致系统状态估计结果远偏离预期，此时需要对卡尔曼滤波做一些调整，使其能适用于非高斯的噪声分布。

2 原理

首先有状态转移方程和观测方程如下，与标准卡尔曼滤波中的形式一致。

θk=Aθk−1+skzk=Cθk+vk{{\bf{\theta }}_k} = {\bf{A}}{{\bf{\theta }}_{k - 1}} + {{\bf{s}}_k} \\ {{\bf{z}}_k} = {\bf{C}}{{\bf{\theta }}_k} + {{\bf{v}}_k} θk=Aθk−1+skzk=Cθk+vk

其中θk{\bf{\theta }}_kθk表示第k{k}k时刻的系统状态， zk{\bf{z}}_kzk表示对k{k}k时刻系统状态的观测结果，而A\bf{A}A和C\bf{C}C分别代表状态转移矩阵和观测矩阵，另外sk{\bf{s}}_ksk为状态噪声，sk∼N(0,Q){{\bf{s}}_k}\sim N\left( {{\bf{0}},{\bf{Q}}} \right)sk∼N(0,Q)， vk{{\bf{v}}_k}vk为观测噪声，vk∼N(0,R/wk){{\bf{v}}_k}\sim N\left( {{\bf{0}},{\bf{R}}/w_k} \right)vk∼N(0,R/wk)，权重 wk∼Gamma(a,b){{w}_k}\sim Gamma\left( {a,b} \right)wk∼Gamma(a,b)。这里Q∈RN×N{\bf{Q}} \in {\mathbb{R}^{N \times N}}Q∈RN×N是一个对角阵，对角线上的元素为q∈RN×1{\bf{q}} \in {\mathbb{R}^{N \times 1}}q∈RN×1，R∈RM×M{\bf{R}} \in {\mathbb{R}^{M \times M}}R∈RM×M也是一个对角阵，对角阵上的元素为r∈RM×1{\bf{r}} \in {\mathbb{R}^{M \times 1}}r∈RM×1。

对于标准的卡尔曼滤波而言，矩阵Q{\bf{Q}}Q和矩阵R{\bf{R}}R表征的是状态噪声和观测噪声的协方差，而卡尔曼滤波算法的最终状态估计，说到底就是根据矩阵Q{\bf{Q}}Q和矩阵R{\bf{R}}R的值去权衡系统的估计偏向预测和偏向观测的程度。举个例子，如果矩阵Q{\bf{Q}}Q远大于R{\bf{R}}R矩阵，也就是说状态噪声十分剧烈，相对的观测噪声很小，那么系统的估计就非常接近观测的结果；反之矩阵Q{\bf{Q}}Q远小于矩阵R{\bf{R}}R，也就是说观测噪声相当大，相对的状态噪声很小，那么系统的状态估计就更多地偏向于根据上一个时刻的状态估计所预测的当前时刻系统状态。

我们可以将这样的模型及求解模型的问题视为期望最大化（EM）学习问题，然后通过最大化对数似然函数，学习这些变量的值。有似然函数为

ln⁡p(θ0:k,z1:k,w1:k)=∑i=1kln⁡p(zi∣θi,wi)+∑i=1kln⁡p(θi∣θi−1)+ln⁡p(θ0)+∑i=1kln⁡p(wi)=12∑i=1kln⁡wi−k2ln⁡∣R∣−k2ln⁡∣Q∣−12∑i=1kwi(zi−Cθi)TR−1(zi−Cθi)−12∑i=1kln⁡wi−12∑i=1k[θi−Aθi−1]TQ−1[θi−Aθi−1]−12ln⁡∣Q0∣−12(θ0−θ^0)Q0−1(θ0−θ^0)+∑i=1kaln⁡wi−∑i=1kbwi+const\ln p({{\bf{\theta }}_{0:k}},{{\bf{z}}_{1:k}},{w_{1:k}}) = \sum\limits_{i = 1}^k {\ln } p\left( {{{\bf{z}}_i}|{{\bf{\theta }}_i},{w_i}} \right) + \sum\limits_{i = 1}^k {\ln } p\left( {{{\bf{\theta }}_i}|{{\bf{\theta }}_{i - 1}}} \right) + \ln p\left( {{{\bf{\theta }}_0}} \right) + \sum\limits_{i = 1}^k {\ln } p\left( {{w_i}} \right) \\ = \frac{1}{2}\sum\limits_{i = 1}^k {\ln {w_i}} - \frac{k}{2}\ln \left| {\bf{R}} \right| - \frac{k}{2}\ln \left| {\bf{Q}} \right| - \frac{1}{2}\sum\limits_{i = 1}^k {{w_i}{{\left( {{{\bf{z}}_i} - {\bf{C}}{{\bf{\theta }}_i}} \right)}^T}{{\bf{R}}^{ - 1}}\left( {{{\bf{z}}_i} - {\bf{C}}{{\bf{\theta }}_i}} \right)} \\- \frac{1}{2}\sum\limits_{i = 1}^k {\ln } {w_i} - \frac{1}{2}\sum\limits_{i = 1}^k {{{\left[ {{{\bf{\theta }}_i} - {\bf{A}}{{\bf{\theta }}_{i - 1}}} \right]}^T}{{\bf{Q}}^{ - 1}}\left[ {{{\bf{\theta }}_i} - {\bf{A}}{{\bf{\theta }}_{i - 1}}} \right]} - \frac{1}{2}\ln \left| {{{\bf{Q}}_0}} \right| \\- \frac{1}{2}\left( {{{\bf{\theta }}_0} - {{{\bf{\hat \theta }}}_0}} \right){\bf{Q}}_0^{ - 1}\left( {{{\bf{\theta }}_0} - {{\widehat {\bf{\theta }}}_0}} \right) + \sum\limits_{i = 1}^k {a\ln {w_i}} - \sum\limits_{i = 1}^k b {w_i} + {\rm{const}} lnp(θ0:k,z1:k,w1:k)=i=1∑klnp(zi∣θi,wi)+i=1∑klnp(θi∣θi−1)+lnp(θ0)+i=1∑klnp(wi)=21i=1∑klnwi−2kln∣R∣−2kln∣Q∣−21i=1∑kwi(zi−Cθi)TR−1(zi−Cθi)−21i=1∑klnwi−21i=1∑k[θi−Aθi−1]TQ−1[θi−Aθi−1]−21ln∣Q0∣−21(θ0−θ^0)Q0−1(θ0−θ0)+i=1∑kalnwi−i=1∑kbwi+const

我们可以从正态和Gamma分布的标准形式中得出最终的EM更新方程，即对于当前时间步k{k}k，有如下算法：
E-Step：
Σk=⟨wk⟩(CkTRk−1Ck+Qk−1)−1{{\bf{\Sigma }}_k} = \left\langle {{w_k}} \right\rangle {\left( {{\bf{C}}_k^T{\bf{R}}_k^{ - 1}{{\bf{C}}_k} + {\bf{Q}}_k^{ - 1}} \right)^{ - 1}}Σk=⟨wk⟩(CkTRk−1Ck+Qk−1)−1

⟨θk⟩=Σk(Qk−1Ak⟨θk−1⟩+⟨wk⟩CkTRk−1zk)\left\langle {{{\bf{\theta }}_k}} \right\rangle = {{\bf{\Sigma }}_k}\left( {{\bf{Q}}_k^{ - 1}{{\bf{A}}_k}\left\langle {{{\bf{\theta }}_{k - 1}}} \right\rangle + \left\langle {{w_k}} \right\rangle {\bf{C}}_k^T{\bf{R}}_k^{ - 1}{{\bf{z}}_k}} \right) ⟨θk⟩=Σk(Qk−1Ak⟨θk−1⟩+⟨wk⟩CkTRk−1zk)

⟨wk⟩=a+12b+12(zk−Cθk)TRk−1(zk−Cθk)\left\langle {{w_k}} \right\rangle = \frac{{a + \frac{1}{2}}}{{b + \frac{1}{2}{{\left( {{{\bf{z}}_k} - {\bf{C}}{{\bf{\theta }}_k}} \right)}^T}{\bf{R}}_k^{ - 1}\left( {{{\bf{z}}_k} - {\bf{C}}{{\bf{\theta }}_k}} \right)}} ⟨wk⟩=b+21(zk−Cθk)TRk−1(zk−Cθk)a+21

M-Step：

Ck=(∑i=1k⟨wi⟩zi⟨θi⟩T)(∑i=1k⟨wi⟩⟨θiθiT⟩)−1{{\bf{C}}_k} = \left( {\sum\limits_{i = 1}^k {\left\langle {{w_i}} \right\rangle {{\bf{z}}_i}} {{\left\langle {{{\bf{\theta }}_i}} \right\rangle }^T}} \right){\left( {\sum\limits_{i = 1}^k {\left\langle {{w_i}} \right\rangle } \left\langle {{{\bf{\theta }}_i}{\bf{\theta }}_i^T} \right\rangle } \right)^{ - 1}} Ck=(i=1∑k⟨wi⟩zi⟨θi⟩T)(i=1∑k⟨wi⟩⟨θiθiT⟩)−1

Ak=(∑i=1k⟨θi⟩⟨θi−1⟩T)(∑i=1k⟨θi−1θi−1T⟩)−1{{\bf{A}}_k} = \left( {\sum\limits_{i = 1}^k {\left\langle {{{\bf{\theta }}_i}} \right\rangle {{\left\langle {{{\bf{\theta }}_{i - 1}}} \right\rangle }^T}} } \right){\left( {\sum\limits_{i = 1}^k {\left\langle {{{\bf{\theta }}_{i - 1}}{\bf{\theta }}_{i - 1}^T} \right\rangle } } \right)^{ - 1}} Ak=(i=1∑k⟨θi⟩⟨θi−1⟩T)(i=1∑k⟨θi−1θi−1T⟩)−1

rkm=1k∑i=1k⟨wi⟩⟨(zim−Ck(m,:)θi)2⟩{r_{km}} = \frac{1}{k}\sum\limits_{i = 1}^k {\left\langle {{w_i}} \right\rangle \left\langle {{{\left( {{{\bf{z}}_{im}} - {{\bf{C}}_k}(m,:){{\bf{\theta }}_i}} \right)}^2}} \right\rangle } rkm=k1i=1∑k⟨wi⟩⟨(zim−Ck(m,:)θi)2⟩

qkn=1k∑i=1k⟨(θin−Ak(n,:)θi−1)2⟩{q_{kn}} = \frac{1}{k}\sum\limits_{i = 1}^k {\left\langle {{{\left( {{{\bf{\theta }}_{in}} - {{\bf{A}}_k}(n,:){{\bf{\theta }}_{i - 1}}} \right)}^2}} \right\rangle } qkn=k1i=1∑k⟨(θin−Ak(n,:)θi−1)2⟩

其中rkm{r_{km}}rkm表示向量rk{{\bf{r}}_k}rk的第mmm个元素，qkn{q_{kn}}qkn表示向量qk{{\bf{q}}_k}qk的第nnn个元素，aaa和bbb是先验的Gamma分布的尺度参数，当kkk时刻的传感器采样结果获得时，便可以进行上述的算法。

⟨wk⟩\left\langle {{w_k}} \right\rangle⟨wk⟩的计算公式中，θk{{\bf{\theta }}_k}θk的值可以用上一个时刻的估计结果⟨θk−1⟩\left\langle {{{\bf{\theta }}_{k - 1}}} \right\rangle⟨θk−1⟩ 所预测的当前时刻状态值θk′=A⟨θk−1⟩{\bf{\theta }}_k^{'}{\rm{ = }}{\bf{A}}\left\langle {{{\bf{\theta }}_{k - 1}}} \right\rangleθk′=A⟨θk−1⟩代替。也就是说，权值⟨wk⟩\left\langle {{w_k}} \right\rangle⟨wk⟩近似的与误差的平方成反比，假定(k−1)(k-1)(k−1)时刻的状态估计⟨θk−1⟩\left\langle {{{\bf{\theta }}_{k - 1}}} \right\rangle⟨θk−1⟩是符合预期的，那么如果kkk时刻的传感器观测出现奇异值的话，误差就会取大值，因此权值⟨wk⟩\left\langle {{w_k}} \right\rangle⟨wk⟩很小，进而观测噪声的协方差矩阵就会变大，根据卡尔曼滤波的原理，对当前时刻的状态做估计时，结果会更偏向于预测而比较不会被观测到的奇异值所影响。

另外，算法对矩阵Q{\bf{Q}}Q和矩阵R{\bf{R}}R的初始值设置较为敏感，其值应该根据观测数据的噪声情况设定，例如对于观测噪声较大的数据，可以取值为Q=R=0.01I{\bf{Q}}{\rm{ = }}{\bf{R}} = 0.01{\bf{I}}Q=R=0.01I，对于观测噪声较小的数据，可以取初始值为Q=R=0.0001I{\bf{Q}}{\rm{ = }}{\bf{R}} = 0.0001{\bf{I}}Q=R=0.0001I，其中I{\bf{I}}I表示单位阵。

以上算法与卡尔曼滤波的关系可以通过联合两个算法的公式获得，因此奇异值鲁棒的卡尔曼滤波算法也可以通过下列公式计算：
预测：

θk′=Ak⟨θk−1⟩{\bf{\theta }}_k^ {'}= {{\bf{A}}_k}\left\langle {{{\bf{\theta }}_{k - 1}}} \right\rangle θk′=Ak⟨θk−1⟩

Σk′=Qk{\bf{\Sigma }}_k^{'}= {{\bf{Q}}_k}Σk′=Qk

更新：

Sk=(CkΣk′CkT+1wkRk)−1{{\bf{S}}_k} = {\left( {{{\bf{C}}_k}{\bf{\Sigma }}_k^{'}{\bf{C}}_k^T + \frac{1}{{{w_k}}}{{\bf{R}}_k}} \right)^{ - 1}}Sk=(CkΣk′CkT+wk1Rk)−1

Kk=Σk′CkTSk{{\bf{K}}_k} = {\bf{\Sigma }}_k^{'}{\bf{C}}_k^T{{\bf{S}}_k}Kk=Σk′CkTSk

⟨θk⟩=θk′+Kk(zk−Ckθk′)\left\langle {{{\bf{\theta }}_k}} \right\rangle = {\bf{\theta }}_k^{'} + {{\bf{K}}_k}\left( {{{\bf{z}}_k} - {{\bf{C}}_k}{\bf{\theta }}_k^{'}} \right)⟨θk⟩=θk′+Kk(zk−Ckθk′)

Σk=(I−KkCk)Σk′{{\bf{\Sigma }}_k} = \left( {{\bf{I}} - {{\bf{K}}_k}{{\bf{C}}_k}} \right){\bf{\Sigma }}_k^{'}Σk=(I−KkCk)Σk′

在进行上述算法之前，可以先用EM算法估计各个参数的值，包括其中的权值wk{w_k}wk，当观测出现奇异值时， wk{w_k}wk的参数估计结果就会很小，于是矩阵Sk{{\bf{S}}_k}Sk的值就会减小，从而导致卡尔曼增益 Kk{{\bf{K}}_k}Kk减小，于是Kk(zk−Ckθk′){{\bf{K}}_k}\left( {{{\bf{z}}_k} - {{\bf{C}}_k}{\bf{\theta }}_k^{'}} \right)Kk(zk−Ckθk′)就会减小，最终导致系统状态的估计结果偏向于θk′{\bf{\theta }}_k^{'}θk′，即对当前时刻的状态预测值θk′=Ak⟨θk−1⟩{\bf{\theta }}_k^{'} = {{\bf{A}}_k}\left\langle {{{\bf{\theta }}_{k - 1}}} \right\rangleθk′=Ak⟨θk−1⟩。

3 实验

在12自由度（DOF）机器狗上评估了所有滤波器，如下图所示。该机器狗有两个测量其方向的信号源：运动捕获（MOCAP）系统和机载惯性测量单位（IMU）。两者都提供了机器人方向的四元数q：来自MOCAP的qMOCAP和来自IMU的qIMU。由于IMU无法提供稳定的方向估计，但其信号干净，因此qIMU随时间漂移。在IMU中发生的漂移在传感器收集需要集成的数据的系统中非常普遍。相比之下，qMOCAP具有奇异值和噪声，但没有漂移。我们想估计qMOCAP和qIMU之间的偏移，该偏移是一个包含奇异值的有噪声的缓慢漂移信号。

图中可以看出，观测结果随机出现偏离正常波动范围的奇异值，传统的卡尔曼滤波算法并不具有检测奇异值的功能，观测到奇异值时，算法认为该时刻的观测仍然是有效的，认为奇异值反映了系统状态出现了较大的改变，所以给出了偏离正常范围的系统状态估计结果。而实际上，奇异的观测并非系统状态的有效反映，而是噪声出现离群值，对于这样的噪声，奇异值鲁棒的加权卡尔曼滤波算法通过结合上一个时刻的状态估计以及当前时刻的观测结果，计算权值，并在估计当前时刻的状态时，用该权值修正新息，抑制奇异值的影响，从而获得更鲁棒的估计结果。

同理于图3所示的观测数据滤波效果对比。

4 参考文献

[1] . Joanne Ting，Evangelos Theodorou，Stefan Schaal. A Kalman Filter for Robust Outlier Detection. 2008

原创性声明：本文属于作者原创性文章，小弟码字辛苦，转载还请注明出处。谢谢~

如果有哪些地方表述的不够得体和清晰，有存在的任何问题，亦或者程序存在任何考虑不周和漏洞，欢迎评论和指正，谢谢各路大佬。

有需要相关技术支持的可咨询QQ：297461921

卡尔曼滤波系列（五）——奇异值鲁棒的卡尔曼滤波算法相关推荐

可完全分离的二维矢量图加密域鲁棒可逆水印算法（一）
摘要随着云制造技术的兴起,加密域可逆水印技术逐渐受到了较多的关注.然而,现有的大部分算法不仅只能应用于图像.视频等冗余性较大的载体,而且难以抵御常见的攻击,甚至只能在单一的域中提取水印.为此,本文针 ...
Patchwork++论文阅读——基于3D点云的快速鲁棒地面分割算法
文章目录摘要 1. 介绍 2. 相关工作 A. 基于学习的地面分割方法 B. 传统的地面分割方法 C. 地面分割的应用 3. PATCHWORK++:快速.稳健.自适应的地面分割 A. 问题定义 B ...
Boosting和Bagging: 如何开发一个鲁棒的机器学习算法
https://www.toutiao.com/a6702386122678338062/ 2019-06-14 22:01:14 作者:Ben Rogojan编译:ronghuaiyang 导读机 ...
as算法 matlab,APAP（As Projective As Possible）视差鲁棒的图像拼接算法
[实例简介] 论文<As-Projective-As-Possible Image Stitching with Moving DLT>中的拼接算法,对于视差图像拼接具有一定的鲁棒性,但是 ...
TeaseR++：快速鲁棒的C++点云配准库介绍+英文版视频教程
本文提出了一种快速鲁棒的点云配准算法,对存在离群噪声点的点云数据具有较好的配准效果.首先使用了截断最小二乘(Truncated Least Squares TLS)代价函数重新构造配准问题 ,该代价是 ...
【论文摘要】基于多数投票模式和超混沌加密的彩色图像鲁棒安全零水印算法
Robust and secure zero-watermarking algorithm for color images based on majority voting pattern and ...
【源头活水】IEEE TIFS 2022 | 基于不确定因素感知的鲁棒虹膜识别
"问渠那得清如许,为有源头活水来",通过前沿领域知识的学习,从其他研究领域得到启发,对研究问题的本质有更清晰的认识和理解,是自我提高的不竭源泉.为此,我们特别精选论文阅读笔记,开辟 ...
matlab鲁棒优化算法,鲁棒优化算法的研究及应用.pdf
论文题目:鲁棒优化算法研究及应用溉一学科名称:计算数学研究生:黄姣茹签名: 指导教师:钱富才教授签名: 摘要在很多实际问题中,数据的不确定性无处不在.例如,在供应链优化问题中,在需要做 ...
卡尔曼滤波系列——（四）无损卡尔曼滤波
1 简介无损卡尔曼滤波又称无迹卡尔曼滤波(Unscented Kalman Filter,UKF),是无损变换(Unscented Transform,UT)与标准卡尔曼滤波体系的结合,通过UT变换 ...

卡尔曼滤波系列（五）——奇异值鲁棒的卡尔曼滤波算法

目录

1 简介

2 原理

3 实验

4 参考文献

卡尔曼滤波系列（五）——奇异值鲁棒的卡尔曼滤波算法相关推荐

最新文章

热门文章