随着时间推移， $d(t)→0$，当 $d(t)=0$ 时，还原为普通的卡尔曼滤波器

1. 更新 $\overline{q}_k, \overline{Q}_k,\overline{r}_k,\overline{R}_k$

扩展卡尔曼滤波 EKF

用来解决非线性问题，如极坐标系的雷达，观测到的式径向距离和角度，这是观测矩阵 H 就是非线性的函数。

EKF 与 KF 最主要的区别就是转换模型和观测模型可以是非线性的，可以使用泰勒展开式替换为线性函数，两个协方差矩阵 P 和 H 要使用雅各比矩阵计算每个状态量的一阶偏导

因为主要是状态估计方程和状态估计转移方程的两个地方有些区别，所以 EKF 协方差矩阵的公式推导还是跟 KF 一样的

预测

使用 Jacobians 矩阵更新模型

过程模型(矩阵)和测量模型(矩阵)与 KF 有所区别，需要进行雅各比矩阵求导运算

其中

更新

恒定转弯率和速度模型（**Constant Turn Rate and Velocity——**CTRV）

状态量为

状态方程为

其中在某一时刻将速度近似于常量，同时角速度也近似于常量。

对上式进行简化

则，上式的最终状态方程为

对其中每个状态量求偏导得到雅各比矩阵 $J_F$

对于观测矩阵，如果是线性的，不用做计算。如果是非线性的，同样可以使用求偏导数的方式计算出雅各比矩阵 $J_H$

假设传感器是毫米波雷达观，且观测量是

上式对状态方程每个状态量求偏导，得到雅各比矩阵，即观测矩阵 $J_H$ 为

Q矩阵计算

对于 CTRV，假设直线加速度 $a_a$ 和偏航角加速度 $a_{\omega}$ 为常数，但是实际过程中会有噪声，因此 CTRV 的模型过程噪声主要来自于 $a_a$ 和 $a_\omega$。假设该模型噪声都符合高斯分布，即

则这两个加速度对状态量的预测噪声为

则

带入之后得

常用的CV模型的状态量为

其状态方程为

该式子对状态量中的每个参数求偏导，得到 $J_F$

如果假设观测量为

该式子对系统状态量中每个参数求偏导，得到 $J_H$

Q矩阵计算

对于 CV，假设直线加速度 $a_x$ 和偏航角加速度 $a_{y}$ 为常数，但是实际过程中会有噪声，因此 CV 的模型过程噪声主要来自于 $a_x$ 和 $a_y$。假设该模型噪声都符合高斯分布，即

则

恒定转弯率和加速度模型（**Constant Turn Rate and Velocity——**CTRA）——过于复杂

其中的系统状态量为

系统状态方程为

其中 $a$ 和 $\omega$ 为常量，与时间无关，其中 $\Delta t=t_{k+1}-t_k$，其中积分化简为

所以上式最终可以表示为

对状态方程的每个状态量求偏导，得到雅各比矩阵 $J_F$

无迹卡尔曼滤波 UKF

卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

无迹卡尔曼滤波（Unscented Kalman Filter，UKF），是无损变换（Unscented Transform，UT变换）与标准卡尔曼滤波体系的结合，通过无损变换变换使非线性系统方程适用于线性假设下的标准卡尔曼体系，相当于是 KF 基础上加入了 UT 变换

对于非线性问题的处理，过程模型F和测量模型H是非线性的，EKF是求一阶全导数得到线性模型，来近似非线性模型；而UKF是直接寻找一个与真实分布近似的高斯分布，没有用线性表征。

使用背景

• 对于某个系统，你拥有准确的数学模型（状态方程和观测方程），也就是说，给出这个系统的输入，你必然能算出这个系统的输出；但是，在现实生活中往往拿到的第一手测试数据并不是你想要的最直观的数据（而且数据还有噪声），这时候使用UKF可以依靠相对准确的数值模型和结合测试数据来得到你想要的信息
• 数值模型的参数相对不准确，但实测数据可信度更好，这时候使用UKF可以依靠相对准确的实测数据和结合数值模型来还原较真实的参数

核心思想

该算法的核心思想是：采用UT变换，利用一组Sigma采样点来描述随机变量的高斯分布，然后通过非线性函数的传递，再利用加权统计线性回归技术来近似非x线性函数的后验均值和方差。相比于EKF，UKF的估计精度能够达到泰勒级数展开的二阶精度

UKF的原理与实现流程

1. UT变换

   例如函数 $y=f(x)$ 是一个非线性函数，x 为 n 维随机变量，它的均值为 $\overline{x}$，方差为 $P_x$。UT变换根据一定的采样策略，获得一组 Sigma 采样点 $\{x_i\},i=0,1,…,L$，并且设定均值权值 $\omega_m^{(i)}$ 和方差权值 $\omega_c^{(i)}$，来近似非线性函数的后验均值和方差，利用选取的 Sigma 采样点集 $\{x_i\},i=0,1,…,L$，进行非线性函数传递可得 $y_i=f(x_i)$，其中 $y_i$ 为 Sigma 采样经过非线性函数传递后对应的点。根据加权统计线性回归技术可以近似得到 y 的统计特性：

   $$ \overline{y}=\sum_{i=0}^{L}{\omega_m^{(i)}y_i}\\P_{yy}=\sum_{i=0}^{L}{\omega_c^{(i)}(y_i-\overline{y})(y_i-\overline{y})^T}\\P_{xy}=\sum_{i=0}^{L}{\omega_c^{(i)}(x_i-\overline{x})(y_i-\overline{y})^T} $$

2. 采样策略

   根据 Sigma 点采样策略不同，相应的 Sigma 点以及均值权值和方差权值也不尽相同，因此 UT 变换的估计精度也会有差异，但总体来说，其估计精度能够达到泰勒级数展开的二阶精度。为保证随机变量x经过采样之后得到的 Sigma 采样点仍具有原变量的必要特性，所以采样点的选取应满足下列条件：

   $$ g(\{x_i,\omega_m^{(i)},\omega_c^{(i)}\},L,P_x(x))=0 $$

   其中

   • $\{x_i,\omega_m^{(i)},\omega_c^{(i)}\}$ 为 Sigma 采样点，均值权重和方差权重组合的集合
   • L 为采样点个数
   • $P_x(x)$ 为随机变量 x 的密度函数
   • $g()$ 确定 x 的相关信息。如果密度函数 $P_x(x)$ 只有一二阶矩时可以写成
   $$ g(\{x_i,\omega_m^{(i)},\omega_c^{(i)}\},L,P_x(x))=\begin{bmatrix}\sum_{i=0}^{L}\omega_m^{(i)}-1\\\sum_{i=0}^{L}\omega_c^{(i)}-1\\\sum_{i=0}^{L}w_m^{(i)}x_i-\overline{x}\\\sum_{i=0}^{L}{\omega_c^{(i)}(x_i-\overline{x})(x_i-\overline{x})^T}-P_{xx}\end{bmatrix}=0 $$

   采样策略：比例采样

   n 维随机变量 x 的均值和方差分别为 $\overline{x}$ 和 $P_x$，假设 L=2n，则 Sigma 采样点共有 2n+1 个，对应的采样点和均值权值和方差权值

   $$ X^{(i)}=\left\{\begin{aligned}&\overline{X}&&i=0\\&\overline{X}+(\sqrt{(n+\kappa )P})_i&&i=1\sim n\\&\overline{X}-(\sqrt{(n+\kappa) P})_i&&i=n+1\sim 2n\end{aligned}\right.\\\omega_m^{(i)}=\omega_c^{(i)}=\left\{\begin{aligned}&\frac{\kappa }{n+\kappa }&&i=0\\&\frac{\kappa }{2(n+\kappa )}&&i=1\sim 2n\end{aligned}\right. $$

   其中

   • $\kappa$ 定义为随机变量 x 的均值 $\overline{x}$ 和 Sigma 采样点间距离的比例因子，其取值的大小直接反应二阶以上的高阶矩引起的偏差
   • $(\sqrt{(n+\kappa) P})_i$ 定义为矩阵 $(n+\kappa) P$ 经过 $Cholesky$ 分解求得的平方根矩阵的第 i 行或列

   采样策略：比例修正对称采样

   为保证协方差的半正定性以及解决采样的非局部效应问题，采用比例修正采样策略来近似非线性函数的后验分布

   • 比例修正算法

     $$ x_i'=x_0+\alpha(x_i-x_0)\\(\omega_m^{(i)})'=\left\{\begin{aligned}&\frac{\omega_m^{(i)}}{\alpha^2}+1-\frac{1}{\alpha^2}&&i=0\\&\frac{\omega_m^{(i)}}{\alpha^2}&&i\not=0\end{aligned}\right.\\(\omega_c^{(i)})'=\left\{\begin{aligned}&(\omega_m^{(i)})'+(1+\beta-\alpha^2)&&i=0\\&(\omega_m^{(i)})'&&i\not=0\end{aligned}\right. $$

     其中 $\alpha$ 为比例缩放因子， $0\leq\alpha\leq 1$，控制 $\alpha$ 的值可以控制 Sigma 点集的范围，通常设置为一个很小的正数。 $\beta$ 反映状态历史信息的高阶特性，对于高斯分布来说 $\beta=2$ 为最优

   • 将上述比例修正算法带入对称采样中，得到比例修正对称采样策略

     $$ (X^{(i)})'=\left\{\begin{aligned}&\overline{X}&&i=0\\&\overline{X}+(\sqrt{(n+\lambda )P})_i&&i=1\sim n\\&\overline{X}-(\sqrt{(n+\lambda)P})_i&&i=n+1\sim 2n\end{aligned}\right.\\(\omega_m^{(i)})'=\left\{\begin{aligned}&\frac{\lambda}{n+\lambda}&&i=0\\&\frac{\lambda}{2(n+\lambda)}&&i=1\sim 2n\end{aligned}\right.\\(\omega_c^{(i)})'=\left\{\begin{aligned}&\frac{\lambda}{n + \lambda}+(1-\alpha^2+\beta)&&i=0\\&\frac{\lambda}{2(n+\lambda)}&&i=1\sim 2n\end{aligned}\right. $$

     其中 $\lambda=\alpha^2(n+\kappa)-n$， $\kappa$ 为比例因子，当 x 为单变量时， $\kappa=0$，当 x 为多变量时 $\kappa = 3 - n$。通常需要保证矩阵 $(n+\lambda)P$ 为半正定矩阵

3. 状态方程与观测方程

   $$ X_{k+1}=f(X_k)+w_k\\Z_k=h(X_k)+v_k $$

   其中，

   • $f$ 是系统非线性状态方程函数，即系统在第 k+1 步与第 k 步的数学关系
   • $h$ 是非线性观测方程函数，即当前系统状态与实际观测数据之间的关系
   • $w,v$ 是状态方程与观测方程的当前噪声

4. 获得 2n+1 个采样组合（即Sigma点集）及其权值（n为需要估计的参数数量）

   $$ X^{(i)}=\left\{\begin{aligned}&\overline{X}&&i=0\\&\overline{X}+(\sqrt{(n+\lambda )P})_i&&i=1\sim n\\&\overline{X}-(\sqrt{(n+\lambda)P})_i&&i=n+1\sim 2n\end{aligned}\right. $$

   其中 $(\sqrt{P})^T(\sqrt{P})=P$， $(\sqrt{P})_i$ 表示矩阵方根的第 i 列，其中 P 指的是当前状态的协方差矩阵（每步实时更新）

   对上式进行整理

   $$ X^{(i)}_{k|k}=\begin{bmatrix}\hat{X}_{k|k}\\\hat{X}_{k|k}+(\sqrt{(n+\lambda)P_{k|k}})_i\\\hat{X}_{k|k}-(\sqrt{(n+\lambda)P_{k|k}})_i\end{bmatrix} $$

5. 把这2n+1个点代进状态方程，获得了这些点的k+1步预测

   $$ X^{(i)}_{k+1|k}=f(k, X^{(i)}_{k|k}) $$

6. 根据上式得到的 2n+1 个预测结果，计算系统状态的 k+1 步预测均值和协方差矩阵

   设计计算权重值

   $$ \omega_m^{(i)}=\left\{\begin{aligned}&\frac{\lambda}{n+\lambda}&&i=0\\&\frac{\lambda}{2(n+\lambda)}&&i=1\sim 2n\end{aligned}\right.\\\omega_c^{(i)}=\left\{\begin{aligned}&\frac{\lambda}{n + \lambda}+(1-\alpha^2+\beta)&&i=0\\&\frac{\lambda}{2(n+\lambda)}&&i=1\sim 2n\end{aligned}\right. $$

   其中 m 表示均值，c 表示协方差，上标为几个采样点，其中

   • $\lambda = \alpha^2(n+\kappa)-n$ 是一个缩放比例参数，用来降低总的预测误差
   • $\alpha$ 的选取控制了 Sigma点集的范围，比例缩放因子， $0\leq\alpha\leq1$，通常设置一个很小的正数
   • $\kappa$ 为一个待选参数，当 x 为单变量时， $\kappa=0$，当 x 为多变量时 $\kappa = 3 - n$。通常需要保证矩阵 $(n+\lambda)P$ 为半正定矩阵
   • $\beta$ 是一个非负的权系数，反映状态历史信息的高阶特性，对于高斯分布 $\beta =2$ 最优。可以合并方程中高阶项的动差，这样就可以把高阶项影响包括在内。

   下式的系统状态量的 k+1 步预测均值和协方差矩阵

   $$ \hat{X}_{k+1|k}=\sum_{i=0}^{2n}\omega_m^{(i)}X^{(i)}_{k+1|k}\\P_{k+1|k}=\sum_{i=0}^{2n}\omega_c^{(i)}[X^{(i)}_{k+1|k}-\hat{X}_{k+1|k}][X^{(i)}_{k+1|k}-\hat{X}_{k+1|k}]^T+Q $$

7. 根据上式的预测均值 $\hat{X}_{k+1|k}$ 和协方差矩阵 $P_{k+1|k}$，再次使用 UT 变换，产生新的 2n+1 个 Sigma点集

   $$ X^{(i)}_{k+1|k}=\begin{bmatrix}\hat{X}_{k+1|k}\\(\hat{X}_{k+1|k}+\sqrt{(n+\lambda)P_{k+1|k}})_i\\\hat{X}_{k+1|k}-(\sqrt{(n+\lambda)P_{k+1|k}})_i\end{bmatrix} $$

8. 将上式的点集带入观测方程，得到 k+1 步预测观测量

   $$ Z_{k+1|k}^{(i)}=h(X_{k+1|k}^{(i)}) $$

9. 上式能得到 2n+1 个预测结果，计算系统观测量的 k+1 步预测均值和协方差巨家镇，其中权值还是上面之前的权值

   $$ \overline{Z}_{k+1|k}=\sum_{i=0}^{2n}\omega_m^{(i)}Z_{k+1|k}^{(i)}\\P_{z_kz_k}=\sum_{i=0}^{2n}\omega_c^{(i)}[Z_{k+1|k}^{(i)}-\overline{Z}_{k+1|k}][Z_{k+1|k}^{(i)}-\overline{Z}_{k+1|k}]^T+R\\P_{x_kz_k}=\sum_{i=0}^{2n}\omega_c^{(i)}[X_{k+1|k}^{(i)}-\hat{X}_{k+1|k}][Z_{k+1|k}^{(i)}-\overline{Z}_{k+1|k}]^T $$

10. 计算Kalman增益矩阵

    $$ K_{k+1}=P_{x_kz_k}P_{z_kz_k}^{-1} $$

11. 更新系统状态与协方差 P

    $$ \hat{X}_{k+1|k+1}=\hat{X}_{k+1|k}+K_{k+1}[Z_{k+1}-\overline{Z}_{k+1|k}]\\P_{k+1|k+1}=P_{k+1|k}-K_{k+1}P_{z_kz_k}K^T_{k+1} $$

    其中 $Z_{k+1}$ 是第 k + 1 步实测数据

12. 最终小结

    UKF 用UT变换近似逼近了系统非线性变换后的分布，得到的 Kalman 增益可以类比成状态变量X的在当前步的线性修正率，有了这个线性修正率再去×修正量（实测-预测均值，即 $Z_{k+1}-\overline{Z}_{k+1|k}$ )，从而更新了 k+1 步的状态量，实现了追踪。