卡尔曼滤波系列——（四）无损卡尔曼滤波

最新推荐文章于 2024-06-06 16:51:57 发布

GHelpU

最新推荐文章于 2024-06-06 16:51:57 发布

阅读量1.8w

点赞数 43

分类专栏：信号处理文章标签：无损卡尔曼滤波 UKF 信号处理参数学习（估计）自适应UKF

本文链接：https://blog.csdn.net/weixin_42647783/article/details/89065436

版权

信号处理专栏收录该内容

10 篇文章 121 订阅

订阅专栏

1 简介

无损卡尔曼滤波又称无迹卡尔曼滤波（Unscented Kalman Filter，UKF），是无损变换（Unscented Transform，UT）与标准卡尔曼滤波体系的结合，通过UT变换使非线性系统方程适用于线性假设下的标准卡尔曼体系。

与EKF不同的是，UKF是通过无损变换使非线性系统方程适用于线性假设下的标准Kalman滤波体系，而不是像EKF那样，必须通过线性化非线性函数实现递推滤波。

UKF的基本思想是卡尔曼滤波与无损变换，它能有效地克服EKF估计精度低、稳定性差的问题，因为不用忽略高阶项，所以对于非线性分布统计量的计算精度高。

2 基本算法

2.1 非线性系统状态估计原理

离散系统的状态转移方程和观测方程为

${{\mathbf{\theta }}_{k}}=f({{\mathbf{\theta }}_{k-1}})+{{\mathbf{s}}_{k}}$

${{\mathbf{z}}_{k}}=h({{\mathbf{\theta }}_{k}})+{{\mathbf{v}}_{k}}$

不管条件密度函数 ${{p}_{\mathbf{\theta }|\mathbf{z}}}(\mathbf{\theta }|\mathbf{z})$ 的特性如何，最小均方误差估计就是条件均值 ${{\mu }_{\mathbf{\theta }|\mathbf{z}}}=E[\mathbf{\theta }|\mathbf{z}]$ ，其中 $E[\cdot ]$ 表示期望的计算。

非线性系统的状态估计分成一步预测和测量修正两个步骤：

（1）一步预测

$\mathbf{\theta }_{k}^{'}=E[{{\mathbf{\theta }}_{k}}|{{\mathbf{z}}^{k-1}}]$

根据所有过去时刻的观测信息对状态做最小均方误差做预测，其协方差矩阵为 $\mathbf{\Sigma }_{_{k}}^{'}$

$\mathbf{\Sigma }_{_{k}}^{'}=\left\langle \mathbf{e}_{k}^{'}\mathbf{e}{{_{k}^{'}}^{T}} \right\rangle =\left\langle \left( {{\theta }_{k}}-\theta _{k}^{'} \right){{\left( {{\theta }_{k}}-\theta _{k}^{'} \right)}^{T}} \right\rangle$

（2）观测修正

引入卡尔曼增益 ${{\mathbf{K}}_{k}}$ ，利用实际观测值 $\mathbf{z}_{k}$ 与预测的观测值 $\mathbf{z}_{k}^{'}$ 的差异，对预测值 $\mathbf{\theta }_{k}^{'}$ 进行修正

$\left\langle {{\mathbf{\theta }}_{k}} \right\rangle =E[{{\mathbf{\theta }}_{k}}|{{\mathbf{z}}^{k}}]=\mathbf{\theta }_{k}^{'}+{{\mathbf{K}}_{k}}({{\mathbf{z}}_{k}}-\mathbf{z}_{k}^{'})$

${{\mathbf{K}}_{k}}={{\mathbf{\Sigma }}_{\mathbf{\theta z}}}\left( k \right)\cdot \mathbf{\Sigma }_{\mathbf{z}}^{-1}\left( k \right)$

其中 $\mathbf{z}_{k}^{'}$ ， ${{\mathbf{\Sigma }}_{\mathbf{\theta z}}}\left( k \right)$ 和 $\mathbf{\Sigma }_{\mathbf{z}}^{-1}\left( k \right)$ 的计算公式分别为

$\mathbf{z}_{k}^{'}=E\left[ {{\mathbf{z}}_{k}}|{{\mathbf{Z}}^{k-1}} \right]$

${{\mathbf{\Sigma }}_{\mathbf{\theta z}}}\left( k \right)=E\left\{ \left. \left[ {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right]{{\left[ {{\mathbf{z}}_{k}}-h\left( \mathbf{\theta }_{k}^{'} \right) \right]}^{T}} \right|{{\mathbf{Z}}^{k-1}} \right\}$

${{\mathbf{\Sigma }}_{\mathbf{z}}}\left( k \right)=E\left\{ \left. \left[ {{\mathbf{z}}_{k}}-h\left( \mathbf{\theta }_{k}^{'} \right) \right]{{\left[ {{\mathbf{z}}_{k}}-h\left( \mathbf{\theta }_{k}^{'} \right) \right]}^{T}} \right|{{\mathbf{Z}}^{k-1}} \right\}$

其中 ${{\mathbf{z}}_{k}}$ 表示第 $k$ 次观测向量的值， ${{\mathbf{Z}}^{k-1}}$ 表示前 $(k-1)$ 次观测向量的值的集合。那么估计值与真实值的误差协方差矩阵为

${{\mathbf{\Sigma }}_{k}}=E\left\{ \left. \left[ {{\mathbf{\theta }}_{k}}-\left\langle {{\mathbf{\theta }}_{k}} \right\rangle \right]{{\left[ {{\mathbf{\theta }}_{k}}-\left\langle {{\mathbf{\theta }}_{k}} \right\rangle \right]}^{T}} \right|{{\mathbf{Z}}^{k}} \right\}=\mathbf{\Sigma }_{k}^{'}-{{\mathbf{K}}_{k}}{{\mathbf{\Sigma }}_{\mathbf{z}}}\left( k \right)\mathbf{K}_{k}^{T}$

一般来说，近似非线性函数的概率密度分布比近似非线性函数本身更加容易，因此便有了粒子滤波PF和无迹卡尔曼滤波UKF。粒子滤波通过使用参考分布，随机产生大量粒子，用粒子的分布近似为状态的后验概率密度函数，进而计算得到系统状态的估计，他的主要缺点为粒子数越大计算量就越大，而减少粒子数又会导致整体估计性能下降。而无迹卡尔曼滤波则利用UT变换的思想，使得非线性系统能够适用于线性假设下的卡尔曼滤波体系，其采样形式为确定性采样，在粒子数相对PF较少的情况下，获得较高的逼近精度。

2.2 UT变换

UT变换是用固定数量的参数去近似一个高斯分布，其实现原理为：在原先分布中按某一规则取一些点，使这些点的均值为协方差状态分布与原状态分布的均值和协方差相等；将这些点代入非线性函数中，相应得到非线性函数值点集，通过这些点集可求取变换的均值和协方差。UT变换的实现一般分为以下三个步骤：

（1）构造Sigma点集

根据随机变量 $\mathbf x$ 的统计量均值 $\bar{\mathbf x}$ 和协方差 $\mathbf {P}_{x}$ （如果是标量，则为方差），构造Sigma点集

其中 $\lambda$ 为尺度参数，调整其值可以提高逼近精度， $n$ 为变量 $\mathbf x$ 的维度。用这一组采样点可以近似地表示变量 $\mathbf x$ 的高斯分布。

（2）对Sigma点集做非线性变换

${{\mathbf Y}_{i}}=f({{\mathbf \chi }_{i}}),i=0,1,...,2n$

经过变换后的Sigma点集 $\left\{ {{\mathbf Y}_{i}} \right\}$ 即可近似地表示 $\mathbf{y}=f(\mathbf{x})$ 的分布。

（3）计算函数映射后的均值和方差

对变换后的Sigma点集 $\left\{ {{\mathbf Y}_{i}} \right\}$ 做加权处理，从而得到输出量 $\mathbf y$ 的均值 $\bar{\mathbf y}$ 和方差 $\mathbf {P}_{y}$

$\mathbf{\bar{y}}\approx \sum\limits_{i=1}^{2n}{W_{i}^{(m)}}{{\mathbf{Y}}_{i}}$

${{\mathbf{\bar{P}}}_{y}}\approx \sum\limits_{i=1}^{2n}{W_{i}^{(c)}}({{\mathbf{Y}}_{i}}-\mathbf{\bar{y}}){{({{\mathbf{Y}}_{i}}-\mathbf{\bar{y}})}^{T}}$

权重 $W_{i}^{(m)}$ 和 $W_{i}^{(c)}$ 的计算如下：

$W_{0}^{(c)}=\frac{\lambda }{n+\lambda }+\left( 1-{{\alpha }^{2}}+\beta \right)$ ， $W_{0}^{(m)}=\frac{\lambda }{n+\lambda }$

$W_{i}^{(c)}=W_{i}^{(m)}=\frac{1}{2\left( n+\lambda \right)},i=1,2,...,2n$

其中参数 $\kappa$ 为第二个尺度参数，通常设置为 $0$ 或 $3-n$ ； $\alpha$ 的取值一般在 $[1e^{-4},1)$ 区间内； $\beta$ 为状态分布参数，对高斯分布的变量， $\beta =2$ 最优，如果状态变量是标量的话，则 $\beta =0$ 最优。

以上就是UT变化的三个步骤，其变换的示意图大致如下图所示：

算法的实现流程原理图如下图所示，图中参数 $a=\sqrt{n+\kappa }$ 与相当于本博文中的 $\sqrt{n+\lambda }$ ，希望读者不要误解：

UT变换具有以下特点：

对非线性函数的概率密度分布进行近似，而不是对非线性函数做近似，算法实现不受模型复杂度的影响；
所得到的的非线性函数的统计量的准确性可以达到三阶，而之前EKF的准确性只能达到一阶；
无需计算Jacobi矩阵，这样就能处理不可导的非线性函数。

那么如何把UT变化应用到卡尔曼滤波框架中呢？笔者将在下一节给出。

3 UKF滤波算法

UKF=UT+KF，算法的实现分成两步走：

（1）状态的时间更新

选定状态的 $(2n+1)$ 个Sigma点；

利用UT变换计算后验均值和方差；

（2）状态的观测更新

利用标准的卡尔曼滤波体系更新，但使用的公式有所差异。

根据系统的噪声的存在方式，将其分为加性噪声和隐含噪声，对于两种噪声，UKF滤波的处理方式分为两种，分别是简化的UKF和扩维的UKF，这里仍然假设噪声是高斯分布的。

3.1 简化UKF滤波算法（加性噪声）

系统的状态转移方程和观测方程为

$\small {{\mathbf{\theta }}_{k}}=f({{\mathbf{\theta }}_{k-1}})+{{\mathbf{s}}_{k}}$ ， $\small {{\mathbf{s}}_{k}}\sim \mathcal{N}(0,\mathbf{Q})$

$\small {{\mathbf{z}}_{k}}=h({{\mathbf{\theta }}_{k}})+{{\mathbf{v}}_{k}}$ ， $\small {{\mathbf{v}}_{k}}\sim \mathcal{N}(0,\mathbf{R})$

简化UKF滤波算法的流程为：

（1）初始化

$\left\langle {{\mathbf{\theta }}_{0}} \right\rangle =E\left[ {{\mathbf{\theta }}_{0}} \right]$

${{\mathbf{\Sigma }}_{\mathbf{\theta },0}}=E\left[ \left( {{\mathbf{\theta }}_{0}}-\left\langle {{\mathbf{\theta }}_{0}} \right\rangle \right){{\left( {{\mathbf{\theta }}_{0}}-\left\langle {{\mathbf{\theta }}_{0}} \right\rangle \right)}^{T}} \right]$

（2）状态估计

①计算Sigma点

$\mathbf{\chi }_{k-1}^{0}=\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle ,i=0$

$\mathbf{\chi }_{k-1}^{i}=\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle +{{\left( \sqrt{(n+\lambda )\cdot {{\Sigma }_{\theta ,k-1}}} \right)}_{i}},i=1,...,n$

$\mathbf{\chi }_{k-1}^{i}=\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle -{{\left( \sqrt{(n+\lambda )\cdot {{\Sigma }_{\theta ,k-1}}} \right)}_{i}},i=n+1,...,2n$

②时间传播方程

$\mathbf{\chi }_{k|k-1}^{i}=f\left( \mathbf{\chi }_{k-1}^{i} \right)$

$\theta _{k}^{'}=\sum\limits_{i=0}^{2n}{W_{i}^{(m)}\mathbf{\chi }_{k|k-1}^{i}}$

$\mathbf{\Sigma }_{\mathbf{\theta },k}^{'}=\sum\limits_{i=0}^{2n}{W_{i}^{(c)}\left( \mathbf{\chi }_{k|k-1}^{i}-\mathbf{\theta }_{k}^{'} \right){{\left( \mathbf{\chi }_{k|k-1}^{i}-\mathbf{\theta }_{k}^{'} \right)}^{T}}}+{{\mathbf{Q}}}$

$\mathbf{\gamma }_{k|k-1}^{i}=h\left( \mathbf{\chi }_{k|k-1}^{i} \right)$

$\mathbf{z}_{k}^{'}=\sum\limits_{i=0}^{2n}{W_{i}^{(m)}\mathbf{\gamma }_{k|k-1}^{i}}$

③状态更新方程

${{\mathbf{\Sigma }}_{\mathbf{z},k}}=\sum\limits_{i=0}^{2n}{W_{i}^{(c)}\left( \mathbf{\gamma }_{k|k-1}^{i}-\mathbf{z}_{k}^{'} \right){{\left( \mathbf{\gamma }_{k|k-1}^{i}-\mathbf{z}_{k}^{'} \right)}^{T}}}+{{\mathbf{R}}}$

${{\mathbf{\Sigma }}_{\mathbf{\theta z},k}}=\sum\limits_{i=0}^{2n}{W_{i}^{(c)}\left( \mathbf{\chi }_{k|k-1}^{i}-\mathbf{\theta }_{k}^{'} \right){{\left( \mathbf{\gamma }_{k|k-1}^{i}-\mathbf{z}_{k}^{'} \right)}^{T}}}$

${{\mathbf{K}}_{k}}={{\mathbf{\Sigma }}_{\mathbf{\theta z},k}}\cdot \mathbf{\Sigma }_{\mathbf{z},k}^{-1}$

$\left\langle {{\mathbf{\theta }}_{k}} \right\rangle =\mathbf{\theta }_{k}^{'}+{{\mathbf{K}}_{k}}({{\mathbf{z}}_{k}}-\mathbf{z}_{k}^{'})$

${{\mathbf{\Sigma }}_{\mathbf{\theta },k}}=\mathbf{\Sigma }_{\mathbf{\theta },k}^{'}-{{\mathbf{K}}_{k}}{{\mathbf{\Sigma }}_{\mathbf{z},k}}\mathbf{K}_{k}^{T}$

以上就是简化UKF滤波算法的全部过程。

3.2 扩维UKF滤波算法（噪声隐含）

系统的状态转移方程和观测方程为

${{\mathbf{\theta }}_{k}}=f({{\mathbf{\theta }}_{k-1}},{{\mathbf{s}}_{k}})$ ， $\small {{\mathbf{s}}_{k}}\sim \mathcal{N}(0,\mathbf{Q})$

${{\mathbf{z}}_{k}}=h({{\mathbf{\theta }}_{k}},{{\mathbf{v}}_{k}})$ ， $\small {{\mathbf{v}}_{k}}\sim \mathcal{N}(0,\mathbf{R})$

此时需要对状态变量进行扩展，得到增广状态向量

${{\mathbf{\theta }}_{a,k}}=\left[ {{\mathbf{\theta }}_{k}};{{\mathbf{s}}_{k}};{{\mathbf{v}}_{k}} \right]$

增广状态的均值和协方差矩阵分别为

$\left\langle {{\mathbf{\theta }}_{a,k}} \right\rangle =\left[ \left\langle {{\mathbf{\theta }}_{k}} \right\rangle ;{{\mathbf{0}}_{m\times 1}};{{\mathbf{0}}_{l\times 1}} \right]$

${{\mathbf{\Sigma }}_{\mathbf{a},k}}=\left[ \begin{matrix} {{\mathbf{\Sigma }}_{\mathbf{\theta },k}} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{Q} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{R} \\ \end{matrix} \right]$

其中 $m$ ， $l$ 分别表示状态噪声和观测噪声的维度。

扩维UKF滤波算法的流程为：

（1）初始化

$\left\langle {{\mathbf{\theta }}_{0}} \right\rangle =E\left[ {{\mathbf{\theta }}_{0}} \right]$

$\left\langle {{\mathbf{\theta }}_{\mathbf{a},0}} \right\rangle =E\left[ \left\langle {{\mathbf{\theta }}_{0}} \right\rangle ;{{\mathbf{0}}_{m\times 1}};{{\mathbf{0}}_{l\times 1}} \right]$

${{\mathbf{\Sigma }}_{\mathbf{a},0}}=\left[ \begin{matrix} {{\mathbf{\Sigma }}_{\mathbf{\theta },0}} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{Q} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{R} \\ \end{matrix} \right]$

（2）状态估计

①计算Sigma点

$\mathbf{\chi }_{a,k-1}^{0}=\left\langle {{\mathbf{\theta }}_{a,k-1}} \right\rangle ,i=0$

$\mathbf{\chi }_{a,k-1}^{i}=\left\langle {{\mathbf{\theta }}_{a,k-1}} \right\rangle +{{\left( \sqrt{(n+\lambda )\cdot {{\Sigma }_{a,k-1}}} \right)}_{i}},i=1,...,N$

$\mathbf{\chi }_{a,k-1}^{i}=\left\langle {{\mathbf{\theta }}_{a,k-1}} \right\rangle -{{\left( \sqrt{(n+\lambda )\cdot {{\Sigma }_{a,k-1}}} \right)}_{i}},i=N+1,...,2N$

$N=n+m+l$ 表示增广状态的维度。

${{\mathbf{\chi }}_{\mathbf{a},k}}={{\left[ \mathbf{\chi }_{\mathbf{\theta },k}^{T},\mathbf{\chi }_{\mathbf{s},k}^{T},\mathbf{\chi }_{\mathbf{v},k}^{T} \right]}^{T}}$

②时间传播方程

$\mathbf{\chi }_{\mathbf{\theta },k|k-1}^{i}=f\left( \mathbf{\chi }_{\mathbf{\theta },k-1}^{i},\mathbf{\chi }_{\mathbf{s},k-1}^{i} \right)$

$\theta _{k}^{'}=\sum\limits_{i=0}^{2N}{W_{i}^{(m)}\mathbf{\chi }_{{\theta },k|k-1}^{i}}$

$\mathbf{\Sigma }_{\mathbf{\theta },k}^{'}=\sum\limits_{i=0}^{2N}{W_{i}^{(c)}\left( \mathbf{\chi }_{\mathbf{\theta },k|k-1}^{i}-\mathbf{\theta }_{\mathbf{a},k}^{'} \right){{\left( \mathbf{\chi }_{\mathbf{a},k|k-1}^{i}-\mathbf{\theta }_{\mathbf{a},k}^{'} \right)}^{T}}}$

$\mathbf{\gamma }_{k|k-1}^{i}=h\left( \mathbf{\chi }_{\mathbf{\theta },k|k-1}^{i},\mathbf{\chi }_{\mathbf{v},k-1}^{i} \right)$

$\mathbf{z}_{k}^{'}=\sum\limits_{i=0}^{2N}{W_{i}^{(m)}\mathbf{\gamma }_{k|k-1}^{i}}$

③状态更新方程

${{\mathbf{\Sigma }}_{\mathbf{z},k}}=\sum\limits_{i=0}^{2N}{W_{i}^{(c)}\left( \mathbf{\gamma }_{k|k-1}^{i}-\mathbf{z}_{k}^{'} \right){{\left( \mathbf{\gamma }_{k|k-1}^{i}-\mathbf{z}_{k}^{'} \right)}^{T}}}$

${{\mathbf{\Sigma }}_{\mathbf{\theta z},k}}=\sum\limits_{i=0}^{2N}{W_{i}^{(c)}\left( \mathbf{\chi }_{\theta ,k|k-1}^{i}-\mathbf{\theta }_{k}^{'} \right){{\left( \mathbf{\gamma }_{k|k-1}^{i}-\mathbf{z}_{k}^{'} \right)}^{T}}}$

${{\mathbf{K}}_{k}}={{\mathbf{\Sigma }}_{\mathbf{\theta z},k}}\cdot \mathbf{\Sigma }_{\mathbf{z},k}^{-1}$

$\left\langle {{\mathbf{\theta }}_{k}} \right\rangle =\mathbf{\theta }_{k}^{'}+{{\mathbf{K}}_{k}}({{\mathbf{z}}_{k}}-\mathbf{z}_{k}^{'})$

${{\mathbf{\Sigma }}_{\mathbf{\theta },k}}=\mathbf{\Sigma }_{\mathbf{\theta },k}^{'}-{{\mathbf{K}}_{k}}{{\mathbf{\Sigma }}_{\mathbf{z},k}}\mathbf{K}_{k}^{T}$

以上就是扩维UKF滤波算法的全部过程。

3.3 两类UKF算法比较

根据以上两个小节的内容，简化UKF算法的Sigma点数为 $2n+1$ ,而扩维UKF算法的Sigma点数为 $2N+1=2(n+m+l)+1$ 。可见，处理加性噪声的简化UKF的Sigma点较处理隐含噪声的扩维UKF要少许多，因此计算量也要更少。

4 实际应用

现在我们假设在海上有一艘正在做匀速直线运动的船只，其相对于传感器的横纵坐标为 $(x;y;v_{x};v_{y})$ 为隐藏状态，无法直接获得，而传感器可以测量得到船只相对于传感器的距离和角度 $\small (r;\theta)$ ，传感器采样的时间间隔为 $\Delta t$ ，则：

状态向量 $\small (x;y;v_{x};v_{y})$ ，观测向量 $\small (r;\theta)$

状态转移方程和观测方程为：

$\left[ \begin{matrix} {{x}_{k}} \\ {{y}_{k}} \\ {{v}_{xk}} \\ {{v}_{yk}} \\ \end{matrix} \right]=f(\left[ \begin{matrix} {{x}_{k-1}} \\ {{y}_{k-1}} \\ {{v}_{{{x}_{k-1}}}} \\ {{v}_{{{y}_{k-1}}}} \\ \end{matrix} \right])+{{\mathbf{s}}_{k}}=\left[ \begin{matrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right]\left[ \begin{matrix} {{x}_{k-1}} \\ {{y}_{k-1}} \\ {{v}_{{{x}_{k-1}}}} \\ {{v}_{{{y}_{k-1}}}} \\ \end{matrix} \right]+{{\mathbf{s}}_{k}}$

$\left[ \begin{matrix} {{r}_{k}} \\ {{\theta }_{k}} \\ \end{matrix} \right]=h(\left[ \begin{matrix} {{x}_{k}} \\ {{y}_{k}} \\ {{v}_{xk}} \\ {{v}_{yk}} \\ \end{matrix} \right])+{{\mathbf{v}}_{k}}=\left[ \begin{matrix} \sqrt{x_{k}^{2}+x_{y}^{2}} \\ \arctan \frac{{{y}_{k}}}{{{x}_{k}}} \\ \end{matrix} \right]+{{\mathbf{v}}_{k}}$

这里给定距离传感器的噪声均值为 $0$ ，方差为 $10$ ；角度传感器的噪声均值为 $0$ ，方差为 $0.001$ （单位弧度）；

采样时间点为 $100$ 个， $\alpha =0.01$ ， $\kappa=0$ ， $\beta =2$ ；

船只的初始状态为 $(1000;1500;5;-3)$ ，四个状态量的噪声的方差分别为 $(2;2;0.2;0.2)$ 。仿真结果如下：

从仿真结果可以看出，UKF在这种情形下的滤波效果还是不错的，相比于PF滤波，UKF能更快地跟踪到目标，即收敛速度更快，而且计算的速度几乎跟EKF是一个量级的。但是在实际应用中，对于船只运动的状态转移噪声的均值 $\mathbf q$ 和协方差矩阵 $\mathbf Q$ ，以及传感器的观测噪声的均值 $\mathbf r$ 和协方差矩阵 $\mathbf R$ ，往往都是未知的，有很多情况都只有观测值而已，这样的情形下，就有必要利用观测值对噪声的统计量参数做出适当的估计（学习）。

5 参数估计（学习）

利用EM算法和极大后验概率估计（MAP），对未知的噪声参数做出估计，再利用估计出的参数去递推卡尔曼滤波的解。本文对EM算法在卡尔曼滤波框架中的推导暂时先不给出，之后可能会补充，这里就先给出一种Adaptive-UKF算法的公式。

${{\mathbf{\theta }}_{k}}=f({{\mathbf{\theta }}_{k-1}})+{{\mathbf{s}}_{k}}$ ， ${{\mathbf{s}}_{k}}\sim \mathcal{N}(\mathbf{q},\mathbf{Q})$

${{\mathbf{z}}_{k}}=h({{\mathbf{\theta }}_{k}})+{{\mathbf{v}}_{k}}$ ， ${{\mathbf{v}}_{k}}\sim \mathcal{N}(\mathbf{r},\mathbf{R})$

利用递推的方法：

${{\mathbf{\hat{q}}}_{k+1}}=(1-{{d}_{k+1}}){{\mathbf{\hat{q}}}_{k}}+{{d}_{k+1}}\left[ \left\langle {{\mathbf{\theta }}_{k+1}} \right\rangle -\sum\limits_{i=0}^{2n}{W_{i}^{(m)}f(\mathbf{\chi }_{k}^{i})} \right]$

${{\mathbf{\hat{Q}}}_{k+1}}=(1-{{d}_{k+1}}){{\mathbf{\hat{Q}}}_{k}}+{{d}_{k+1}}\left[ {{\mathbf{K}}_{k+1}}{{\mathbf{e}}_{k+1}}\mathbf{e}_{k+1}^{T}\mathbf{K}_{k+1}^{T}+{{\mathbf{\Sigma }}_{k+1}} \right]$

${{\mathbf{\hat{r}}}_{k+1}}=(1-{{d}_{k+1}}){{\mathbf{\hat{r}}}_{k}}+{{d}_{k+1}}\left[ {{z}_{k+1}}-\sum\limits_{i=0}^{2n}{W_{i}^{(m)}h(\mathbf{\chi }_{k+1|k}^{i})} \right]$

${{\mathbf{\hat{R}}}_{k+1}}=(1-{{d}_{k+1}}){{\mathbf{\hat{R}}}_{k}}+{{d}_{k+1}}{{\mathbf{e}}_{k+1}}\mathbf{e}_{k+1}^{T}$

其中，

${{\mathbf{e}}_{k}}={{\mathbf{z}}_{k}}-\mathbf{z}_{k}^{'}$

${{d}_{k+1}}=\frac{1-b}{1-{{b}^{k+1}}}$

$b$ 的取值一般为 $0.95$ 。

初始的 ${{\mathbf{\hat{Q}}}_{0}}$ ， ${{\mathbf{\hat{R}}}_{0}}$ ， ${{\mathbf{\hat{q}}}_{0}}$ 和 ${{\mathbf{\hat{r}}}_{0}}$ 可以设置为任意较小值，然后每做完一轮UKF滤波，就可以更新一次参数，用于下一轮的UKF滤波。利用以上的Adaptive-UKF算法对船只的运动做滤波跟踪，得到的效果如下图所示：

相比于没有做参数估计的UKF滤波，可以看出，Adaptive-UKF在估计误差上与UKF滤波相差不大，而且，它并不需要指定状态转移噪声和观测噪声的参数，将更有利于在实际中的应用。

6 总结

从整体上看，UKF滤波算法是一个比较优秀的信号滤波与跟踪算法，通过把UT变换应用到卡尔曼滤波框架中，使得不需要计算Jacobi矩阵，不必对非线性函数做任何形式的逼近，即使系统函数不连续，也可以获得不错的滤波性能。而且，UKF对于随机状态的分布不一定要求是高斯分布的，因此UKF具有非常广泛的应用。

UKF与EKF的重要差异是EKF是对高度复杂非线性系统模型函数进行泰勒展开，对展开式进行一阶线性截断处理，这样便可将模型转化为计算机处理的线性问题，然后进行卡尔曼滤波，因此EKF是一种次优滤波，但由于考虑了泰勒级数的展开，因此大大增加了运算量。与对非线性函数的近似相比，高斯分布的近似要简单得多。UKF能获得精确到三阶矩均值和协方差，具有更高的滤波精度，而其计算量仍然与EKF同阶，并且该方法直接使用系统的非线性模型，不需对非线性系统线性化，也不需要像二次滤波那样计算Hession和Jacobi矩阵，提高了运算速度，对线性系统两个具有相同的估计性能。总体来说，对非线性系统，UKF具有更高的滤波精度和稳定性。

7 参考文献

[1] S. Peng, C. Chen, H. Shi and Z. Yao, "State of Charge Estimation of Battery Energy Storage Systems Based on Adaptive Unscented Kalman Filter With a Noise Statistics Estimator," in IEEE Access, vol. 5, pp. 13202-13212, 2017.

[2] 卡尔曼滤波系列——（一）标准卡尔曼滤波.

[3] 卡尔曼滤波系列——（二）扩展卡尔曼滤波.

[4] 卡尔曼滤波系列——（三）粒子滤波（重写）

[5] https://max.book118.com/html/2017/0502/103920556.shtm.

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

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

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

GHelpU

关注

43
点赞
踩
247

收藏

觉得还不错? 一键收藏
17
评论
卡尔曼滤波系列——（四）无损卡尔曼滤波

1 简介无损卡尔曼滤波又称无迹卡尔曼滤波（Unscented Kalman Filter，UKF），是无损变换（Unscented Transform，UT）与标准卡尔曼滤波体系的结合，通过UT变换使非线性系统方程适用于线性假设下的标准卡尔曼体系。与EKF不同的是，UKF是通过无损变换使非线性系统方程适用于线性假设下的标准Kalman滤波体系，而不是像EKF那样，必须通过线性化非线性函数实...
复制链接

扫一扫