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滤波算法（加性噪声）

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

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

${{\mathbf{z}}_{k}}=h({{\mathbf{\theta }}_{k}})+{{\mathbf{v}}_{k}}$ ， ${{\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}})$ ， ${{\mathbf{s}}_{k}}\sim \mathcal{N}(0,\mathbf{Q})$

${{\mathbf{z}}_{k}}=h({{\mathbf{\theta }}_{k}},{{\mathbf{v}}_{k}})$ ， ${{\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})$ 为隐藏状态，无法直接获得，而传感器可以测量得到船只相对于传感器的距离和角度 $(r;\theta)$ ，传感器采样的时间间隔为 $\Delta t$ ，则：

状态向量 $(x;y;v_{x};v_{y})$ ，观测向量 $(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

卡尔曼滤波系列——（四）无损卡尔曼滤波相关推荐

卡尔曼滤波系列——（六）卡尔曼平滑
更新日志: 2020.11.23:修改了2.4节中前向递推的公式书写错误目录 1 简介 2 原理 2.1 概率状态空间模型 2.2 贝叶斯平滑公式 2.3 两个引理 2.4 RTS平滑 3 推导 3 ...
卡尔曼滤波系列——（三）粒子滤波
1 简介粒子滤波(Partical Filter,PF)就是通过寻找一组在状态空间中传播的随机样本来近似的表示概率密度函数,用样本均值代替积分运算,进而获得系统状态的最小方差估计的过程,这些样本被称 ...
无人驾驶汽车系统入门（三）——无损卡尔曼滤波，目标追踪，C++
前面两篇文章我们了解了卡尔曼滤波以及扩展卡尔曼滤波在目标追踪的应用,我们在上一篇文章中还具体用Python实现了EKF,但是细心的同学会发现,EKF的效率确实很低,计算雅可比矩阵确实是一个很费时的操作 ...
无人驾驶技术——无损卡尔曼滤波（UKF）
文章目录无损卡尔曼滤波(UKF) UKF Predict 1.选择预测点sigma点 sigma点计算代码实现添加过程噪声处理构建噪声扩充矩阵 c++ 2.预测sigma点预测sigma点方法 ...
无人驾驶4：无损卡尔曼滤波
在上一节的扩展卡尔曼滤波跟踪系统中,有两个缺陷: 系统采用恒速模型:假定行人沿直线运动:实际应用中,出现曲线运动时,预估不够准确. 每次测量都需要计算雅可比矩阵,很耗资源. 当问题一旦变得复杂,预测和 ...
滤波算法（四）—— 卡尔曼滤波算法
一.算法介绍卡尔曼滤波是一个神奇的滤波算法,应用非常广泛,它是一种结合先验经验.测量更新的状态估计算法. 1.状态估计首先,对于一个我们关心的物理量,我们假设它符合下面的规律其中,为该物理量本周 ...
sed修炼系列(四)：sed中的疑难杂症
sed系列文章: sed修炼系列(一):花拳绣腿之入门篇 sed修炼系列(二):武功心法(info sed翻译+注解) sed修炼系列(三):sed高级应用之实现窗口滑动技术 sed修炼系列(四):s ...
效率系列(四) VS常用快捷键
写在前面的话 :最近博主整理了一些关于 Visual Studio 2017 的常用快捷键,希望可以帮助到大家更高效更愉快的打码 1.打开快捷键描述 Ctrl + Shift + N 新建项目 C ...
iOS开发UINavigation系列四——导航控制器UINavigationController
iOS开发UINavigation系列四--导航控制器UINavigationController 一.引言在前面的博客中,我么你介绍了UINavigationBar,UINavigationIte ...

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