github仓库

卡尔曼滤波 KF

v2-87047c7f01f9c404db864dede8052c16_720w.png

公式

x^k∣k−1=Fkx^k−1∣k−1Pk∣k−1=FkPk−1∣k−1FkT+QkKk=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1x^k∣k=x^k∣k−1+Kk(zk−Hkx^k∣k−1)Pk∣k=(I−KkHk)Pk∣k−1\hat{x}_{k|k-1}=F_k\hat{x}_{k-1|k-1}\\P_{k|k-1}=F_kP_{k-1|k-1}F_k^T+Q_k\\K_k=P_{k|k-1}H_k^T(H_kP_{k|k-1}H_k^T+R_k)^{-1}\\\hat{x}_{k|k}=\hat{x}_{k|k-1}+K_k(z_k-H_k\hat{x}_{k|k-1})\\P_{k|k}=(I-K_kH_k)P_{k|k-1}

基本假设

卡尔曼滤波模型假设k时刻的真实状态是从（k − 1）时刻的状态演化而来

状态估计方程

xk=Fkxk−1+Bkuk+wkx_k=F_kx_{k-1}+B_ku_k+w_k

其中

  • FkF_k 是状态变换模型（矩阵/向量），运动学一般是矩阵（状态转移矩阵）
  • BkB_k 是作用在控制器向量 uku_k 上的输入-控制模型，一般运动学中没有这一项，因对于检测的目标的是无法测量其内部的控制量的，所以简化为0
  • wk−N(0,Qk)w_k-N(0,Q_k) 是过程噪声。均值为 0， xkx_k 对应的就是高斯分布的均值，因此这项可以简化为 0

状态估计转移方程

zk=Hkxk+vkz_k=H_kx_k+v_k

表示 k 时刻真实 xkx_k 的一个测量

其中

  • HkH_k 为观测模型（观测矩阵），它把真实空间状态映射成观测空间状态
  • vk−N(0,Rk)v_k-N(0,R_k) 是观测噪声

初始状态以及每一时刻的噪声 x0,w1,…,wk,v1,…vkx_0,w_1,…,w_k,v_1,…v_k 都被认为是相互独立的

小结

从以上出发，可以计算相应的协方差矩阵，这个也是公式推导的主要部分。

预测

x^k∣k−1=Fkx^k−1∣k−1\hat{x}_{k|k-1}=F_k\hat{x}_{k-1|k-1}

估计状态方程，表示预测下一步状态。

其中

  • x^k∣k−1\hat{x}_{k|k-1} 表示 k-1 时刻对 k 时刻的状态预测
  • x^k−1∣k−1\hat{x}_{k-1|k-1} 表示在 k-1 时刻对 k-1 时刻的状态估计
  • FkF_k 为状态估计矩阵

Pk∣k−1=FkPk−1∣k−1FkT+QkP_{k|k-1}=F_kP_{k-1|k-1}F_k^T+Q_k

预测二估计协方差方程，预测误差方差，计算先验概率

其中

  • Pk−1∣k−1P_{k-1|k-1} 表示 k-1 时刻的后验估计误差协方差矩阵，度量估计值的精确程度
  • Pk∣k−1P_{k|k-1} 表示 k-1 时刻到 k 时刻的估计误差协方差矩阵
  • QkQ_k 表示过程噪声协方差矩阵，越大说明约不相信预测。实际工程中可以自行调参（Qk=cov(Z,Z)Q_k=cov(Z, Z)），也可以自适应AKF

更新

Kk=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1K_k=P_{k|k-1}H_k^T(H_kP_{k|k-1}H_k^T+R_k)^{-1}

卡尔曼增益方程

其中

  • KkK_k 为最优卡尔曼增益
  • RkR_k 为测量噪声协方差矩阵，越大说明越不相信观测。实际测量噪声由厂家提供，或者自己调参，也可自适应AKF

x^k∣k=x^k∣k−1+Kk(zk−Hkx^k∣k−1)\hat{x}_{k|k}=\hat{x}_{k|k-1}+K_k(z_k-H_k\hat{x}_{k|k-1})

更新状态估计方程——最终滤波的效果

可以使用测量残差来简化表达公式，即

y^=zk−Hkx^k∣k−1Sk=cov(y^)=HkPk∣k−1HkT+Rk\hat{y}=z_k-H_k\hat{x}_{k|k-1}\\S_k=cov(\hat{y})=H_kP_{k|k-1}H_k^T+R_k

即卡尔曼增益可以简化为

Kk=Pk∣k−1HkTSk−1K_k=P_{k|k-1}H_k^TS_k^{-1}

状态估计更新可以简化为

x^k∣k=x^k∣k−1+Kky^\hat{x}_{k|k}=\hat{x}_{k|k-1}+K_k\hat{y}

其中 cov(x)cov(x) 求解矩阵的协方差矩阵，若 X 大小为 M×NM \times N，则 cov(X)cov(X) 大小 为 N×NN\times N **的矩阵。 cov(X)cov(X) 的第 (i,j)(i,j) 个元素等于 X 的第 i 列向量与第 j 列向量的方差，即 C(Xi,Xj)C(Xi,Xj)。

Pk∣k=(I−KkHk)Pk∣k−1P_{k|k}=(I-K_kH_k)P_{k|k-1}

更新估计协方差——得到后验概率

公式推导

首先 QkQ_k 和 RkR_k 一般为固定值，高级卡尔曼滤波可以自适应

Qk=cov(wk)=E(wkwkT)Rk=cov(vk)=E(vkvkT)Pk∣k−1=cov(xk−x^k∣k−1)=E((xk−x^k∣k−1)(xk−x^k∣k−1)T)Pk∣k=cov(xk−x^k∣k)=E((xk−x^k∣k)(xk−x^k∣k)T)Sk=cov(y^)=cov(zk−Hkx^k∣k−1)Q_k=cov(w_k)=E(w_kw_k^T)\\R_k=cov(v_k)=E(v_kv^T_k)\\P_{k|k-1}=cov(x_k-\hat{x}_{k|k-1})=E((x_k-\hat{x}_{k|k-1})(x_k-\hat{x}_{k|k-1})^T)\\P_{k|k}=cov(x_k-\hat{x}_{k|k})=E((x_k-\hat{x}_{k|k})(x_k-\hat{x}_{k|k})^T)\\S_k=cov(\hat{y})=cov(z_k-H_k\hat{x}_{k|k-1})

从状态估计方程推导 Pk∣k−1P_{k|k-1}

Pk∣k=cov(xk−x^k∣k)=cov(xk−x^k∣k−1−Kk(zk−Hkx^k∣k−1))=cov(xk−x^k∣k−1−Kk(Hkxk+vk−Hkx^k∣k−1))=cov((I−KkHk)(xk−x^k∣k−1)−Kkvk)=cov((I−KkHk)(xk−x^k∣k−1)+cov(Kkvk)=(I−KkHk)cov(xk−x^k∣k−1)(I−KkHk)T+Kkcov(vk)KkT=(I−KkHk)Pk∣k−1(I−KkHk)T+KkRkKkTP_{k|k}=cov(x_k-\hat{x}_{k|k})\\=cov(x_k-\hat{x}_{k|k-1}-K_k(z_k-H_k\hat{x}_{k|k-1}))\\=cov(x_k-\hat{x}_{k|k-1}-K_k(H_kx_k+v_k-H_k\hat{x}_{k|k-1}))\\=cov((I-K_kH_k)(x_k-\hat{x}_{k|k-1})-K_kv_k)\\=cov((I-K_kH_k)(x_k-\hat{x}_{k|k-1})+cov(K_kv_k)\\=(I-K_kH_k)cov(x_k-\hat{x}_{k|k-1})(I-K_kH_k)^T+K_kcov(v_k)K_k^T\\=(I-K_kH_k)P_{k|k-1}(I-K_kH_k)^T+K_kR_kK_k^T

这一公式对任何卡尔曼增益 KkK_k 都成立，如果 KkK_k 是最优卡尔曼增益，则可以进一步简化

推导最优卡尔曼增益 KkK_k

可以根据上式推导最优卡尔曼增益

卡尔曼滤波器是最小均方误差估计器，后验状态误差估计是指 Pk∣k=cov(xk−x^k∣k)P_{k|k}=cov(x_k-\hat{x}_{k|k})，当 Pk∣kP_{k|k} 矩阵的均方误差为最小时，即可求出最优卡尔曼增益

矩阵的均方误差为矩阵的迹。求矩阵的最小均方误差，即是求矩阵的迹的最小值，对矩阵的迹求导，导数为0时，迹最小。其中协方差 Pk∣kP_{k|k} 是对称矩阵

Pk∣k=(I−KkHk)Pk∣k−1(I−KkHk)T+KkRkKkT=Pk∣k−1−KkHkPk∣k−1−Pk∣k−1(KkHk)T+Kk(HkPk∣k−1HkT+Rk)KkTP_{k|k}=(I-K_kH_k)P_{k|k-1}(I-K_kH_k)^T+K_kR_kK_k^T\\=P_{k|k-1}-K_kH_kP_{k|k-1}-P_{k|k-1}(K_kH_k)^T+K_k(H_kP_{k|k-1}H_k^T+R_k)K_k^T

求解该矩阵的迹

tr(Pk∣k)=tr(Pk∣k−1−KkHkPk∣k−1−Pk∣k−1(KkHk)T+Kk(HkPk∣k−1HkT+Rk)KkT)=tr(Pk∣k−1)−2tr(KkHkPk∣k−1)+tr(Kk(HkPk∣k−1HkT+Rk)KkT)tr(P_{k|k})=tr(P_{k|k-1}-K_kH_kP_{k|k-1}-P_{k|k-1}(K_kH_k)^T+K_k(H_kP_{k|k-1}H_k^T+R_k)K_k^T)\\=tr(P_{k|k-1})-2tr(K_kH_kP_{k|k-1})+tr(K_k(H_kP_{k|k-1}H_k^T+R_k)K_k^T)

上式对 KkK_k 求导

d tr(Pk∣k)d Kk=d[tr(Pk∣k−1)−2tr(KkHkPk∣k−1)+tr(Kk(HkPk∣k−1HkT+Rk)KkT)]d Kk=−2(HkPk∣k−1)T+2Kk(HkPk∣k−1HkT+Rk)\frac{d~tr(P_{k|k})}{d~K_k}=\frac{d[tr(P_{k|k-1})-2tr(K_kH_kP_{k|k-1})+tr(K_k(H_kP_{k|k-1}H_k^T+R_k)K_k^T)]}{d~K_k}\\=-2(H_kP_{k|k-1})^T+2K_k(H_kP_{k|k-1}H_k^T+R_k)

当导数为 0 时取极值，即

−2(HkPk∣k−1)T+2Kk(HkPk∣k−1HkT+Rk)=0-2(H_kP_{k|k-1})^T+2K_k(H_kP_{k|k-1}H_k^T+R_k)=0

得

Kk=(HkPk∣k−1)T(HkPk∣k−1HkT+Rk)−1K_k=(H_kP_{k|k-1})^T(H_kP_{k|k-1}H_k^T+R_k)^{-1}

令

SkHkPk∣k−1HkT+RkS_kH_kP_{k|k-1}H_k^T+R_k

得

Kk=Pk∣k−1HkTSk−1K_k=P_{k|k-1}H_k^TS_k^{-1}

对该矩阵的迹求二阶导

d tr2(Pk∣k)d Kk2=2(HkPk∣k−1Hk−1+Rk)T\frac{d~tr^2(P_{k|k})}{d~K_k^2}=2(H_kP_{k|k-1}H_k^{-1}+R_k)^T

由于 RkR_k 表示 vkv_k 的协方差矩阵，所以其一定是正定阵，因此该矩阵是正定阵，因此此时求出其迹的最小值

最优卡尔曼增益化简 Pk∣kP_{k|k}

从上式化简得到

KkSk=(HkPk∣k−1)TKkSkKkT=(HkPk∣k−1)TKkT=Pk∣k−1(KkHk)TK_kS_k=(H_kP_{k|k-1})^T\\K_kS_kK_k^T=(H_kP_{k|k-1})^TK_k^T=P_{k|k-1}(K_kH_k)^T

带入其中得

Pk∣k=Pk∣k−1−KkHkPk∣k−1−Pk∣k−1(KkHk)T+Kk(HkPk∣k−1HkT+Rk)KkT=Pk∣k−1−KkHkPk∣k−1−Pk∣k−1(KkHk)T+KkSkKkT=(I−KkHk)Pk∣k−1P_{k|k}=P_{k|k-1}-K_kH_kP_{k|k-1}-P_{k|k-1}(K_kH_k)^T+K_k(H_kP_{k|k-1}H_k^T+R_k)K_k^T\\=P_{k|k-1}-K_kH_kP_{k|k-1}-P_{k|k-1}(K_kH_k)^T+K_kS_kK_k^T\\=(I-K_kH_k)P_{k|k-1}

该简化式 Pk∣k=(I−KkHk)Pk∣k−1P_{k|k}=(I-K_kH_k)P_{k|k-1} 只能在最优卡尔曼增益时才成立。如果算术精度总是很低而导致数值稳定性出现问题，或者特意使用非最优卡尔曼增益，则必须使用上面未简化的后验误差协方差公式 Pk∣k=(I−KkHk)Pk∣k−1(I−KkHk)T+KkRkKkTP_{k|k}=(I-K_kH_k)P_{k|k-1}(I-K_kH_k)^T+K_kR_kK_k^T

自适应卡尔曼滤波 AKF

首先列出系统方程

Xk=AXk−1+Cuk+BwkYk=CXk+vkX_k=AX_{k-1}+Cu_k+Bw_k\\Y_k=CX_k+v_k

其中

Xk表示状态量Yk表示输出量uk表示输入量wk,vk为噪声X_k \text{表示状态量}\\Y_k \text{表示输出量}\\u_k \text{表示输入量}\\w_k,v_k \text{为噪声}

E[wk]=qkE[wkwkT]=QkδtpE[vk]=rkE[vkvkT]=RkδtpE[w_k]=q_k\\E[w_kw_k^T]=Q_k\delta_{tp}\\E[v_k]=r_k\\E[v_kv_k^T]=R_k\delta_{tp}

其中 Q(t),R(t),q(t),r(t)Q(t),R(t),q(t),r(t) 将在后续进行更新

具体过程

  1. 预测方程

X^k∣k−1=AX^k−1∣k−1+Cuk−1+Bq‾k−1\hat{X}_{k|k-1}=A\hat{X}_{k-1|k-1}+Cu_{k-1}+B\overline{q}_{k-1}

其中 q‾k−1\overline{q}_{k-1} 表示预测误差矩阵

  1. 预测均方差误差方程

Pk∣k−1=APk−1∣k−1AT+BQ‾k−1BTP_{k|k-1}=AP_{k-1|k-1}A^T+B\overline{Q}_{k-1}B^T

其中 Q‾k−1\overline{Q}_{k-1} 表示过程噪声协方差矩阵，Q越大表示预测的协方差就越大，对预测的不信任

  1. 更新滤波增益方程

Kk=Pk∣k−1CT(CPk∣k−1CT+R‾k−1)−1K_k=P_{k|k-1}C^T(CP_{k|k-1}C^T+\overline{R}_{k-1})^{-1}

其中 R‾k−1\overline{R}_{k-1} 是测量噪声协方差矩阵，R越大表示观测的协方差就越大，对观测不信任

  1. 计算残差

εk=Yk−CX^k∣k−1−r‾k−1\varepsilon_k=Y_k-C\hat{X}_{k|k-1}-\overline{r}_{k-1}

其中 r‾k−1\overline{r}_{k-1} 是观测误差矩阵

  1. 状态估计

X^k∣k=X^k∣k−1+Kkεk\hat{X}_{k|k}=\hat{X}_{k|k-1}+K_k\varepsilon_k

和卡尔曼滤波器的对应公式是一样的

  1. 均方差误差方程

Pk∣k=(I−KkC)Pk∣k−1P_{k|k}=(I-K_kC)P_{k|k-1}

和卡尔曼滤波器的对应公式是一样的

更新 qk,Qk,rk,Rkq_k, Q_k, r_k, R_k

  1. 计算加权系数

d(t)=1−b1−bt+1   0<b<1d(t)=\frac{1-b}{1-b^{t+1}}~~~0<b<1

随着时间的推移， d(t)→1−bd(t)→1-b

d(t)=1td(t)=\frac{1}{t}

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

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

q‾k=(1−d(k−1))q‾k−1+d(k−1)(X^k∣k−AX^k−1∣k−1)Q‾k=(1−d(k−1))Q‾k−1+d(k−1)(KkεkεkTKkT+Pk∣k−APk−1∣k−1AT)r‾k=(1−d(k−1))r‾k−1+d(k−1)(Yk−CX^k∣k−1)R‾k=(1−d(k−1))R‾k−1−d(k−1)(εkεkT−CPk∣k−1CT)\overline{q}_k=(1-d(k-1))\overline{q}_{k-1}+d(k-1)(\hat{X}_{k|k}-A\hat{X}_{k-1|k-1})\\\overline{Q}_k=(1-d(k-1))\overline{Q}_{k-1}+d(k-1)(K_k\varepsilon_k\varepsilon_k^TK_k^T+P_{k|k}-AP_{k-1|k-1}A^T)\\\overline{r}_k=(1-d(k-1))\overline{r}_{k-1}+d(k-1)(Y_k-C\hat{X}_{k|k-1})\\\overline{R}_{k}=(1-d(k-1))\overline{R}_{k-1}-d(k-1)(\varepsilon_k\varepsilon_k^T-CP_{k|k-1}C^T)

扩展卡尔曼滤波 EKF

v2-5d08b454b3149ba0500e647d2bea421f_720w.png

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

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

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

预测

x^k∣k−1=f(x^k−1∣k−1,uk,0)Pk∣k−1=JFPk−1∣k−1JFT+Qk\hat{x}_{k|k-1}=f(\hat{x}_{k-1|k-1},u_k,0)\\P_{k|k-1}=J_FP_{k-1|k-1}J_F^T+Q_k

使用 Jacobians 矩阵更新模型

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

JF=∂f∂x∣x^k−1∣k−1,ukJH=∂h∂x∣x^k∣k−1J_F=\frac{\partial f}{\partial x}|_{\hat{x}_{k-1|k-1}, u_k}\\J_H=\frac{\partial h}{\partial x}|_{\hat{x}_{k|k-1}}

其中

h(x)=Hkxf(x,u,v)=Fkx+Bku+vh(x)=H_kx\\f(x,u,v)=F_kx+B_ku+v

更新

y^k=zk−h(x^k∣k−1,0)Sk=JHPk∣k−1HkT+RkKk=Pk∣k−1JHTSk−1x^k∣k=x^k∣k−1+Kky^Pk∣k=(I−KkJH)Pk∣k−1\hat{y}_k=z_k-h(\hat{x}_{k|k-1}, 0)\\S_k=J_HP_{k|k-1}H_k^T+R_k\\K_k=P_{k|k-1}J_H^TS_k^{-1}\\\hat{x}_{k|k}=\hat{x}_{k|k-1}+K_k\hat{y}\\P_{k|k}=(I-K_kJ_H)P_{k|k-1}

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

状态量为

X=[xyvθω]X=\begin{bmatrix}x\\y\\v\\\theta\\\omega\end{bmatrix}

状态方程为

Xk+1=Xk+[∫tktk+1v(t)cos(θ(t))dt∫tktk+1v(t)sin(θ(t))dt0ωΔt0]X_{k+1}=X_k+\begin{bmatrix}\int_{t_k}^{t_{k+1}}{v(t)cos(\theta(t))}dt\\\int_{t_k}^{t_{k+1}}{v(t)sin(\theta(t))}dt\\0\\\omega \Delta t\\0\end{bmatrix}

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

对上式进行简化

∫tktk+1v(t)cos(θ(t))dt=vk∫tktk+1cos(θk+ω(t−tk))dt=vkω[sin(ωΔ+θ)−sin(θ)]∫tktk+1v(t)sin(θ(t))dt=vk∫tktk+1sin(θk+ω(t−tk))dt=vkω[−cos(ωΔ+θ)+cos(θ)]\int_{t_k}^{t_{k+1}}{v(t)cos(\theta(t))}dt=v_k\int_{t_k}^{t_{k+1}}{cos(\theta_k+\omega (t-t_k))}dt=\frac{v_k}{\omega}[sin(\omega \Delta + \theta)- sin(\theta)]\\\int_{t_k}^{t_{k+1}}{v(t)sin(\theta(t))}dt=v_k\int_{t_k}^{t_{k+1}}{sin(\theta_k+\omega (t-t_k))}dt=\frac{v_k}{\omega}[-cos(\omega \Delta + \theta)+ cos(\theta)]

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

Xk+1=Xk+[vkω[sin(ωΔ+θ)−sin(θ)]vkω[−cos(ωΔ+θ)+cos(θ)]0ωΔt0]=[xk+vkω[sin(ωΔ+θ)−sin(θ)]yk+vkω[−cos(ωΔ+θ)+cos(θ)]vkωΔt+θω]X_{k+1}=X_k+\begin{bmatrix}\frac{v_k}{\omega}[sin(\omega \Delta + \theta)- sin(\theta)]\\\frac{v_k}{\omega}[-cos(\omega \Delta + \theta)+ cos(\theta)]\\0\\\omega \Delta t\\0\end{bmatrix}=\begin{bmatrix}x_k+\frac{v_k}{\omega}[sin(\omega \Delta + \theta)- sin(\theta)]\\y_k+\frac{v_k}{\omega}[-cos(\omega \Delta + \theta)+ cos(\theta)]\\v_k\\\omega \Delta t+\theta\\\omega\end{bmatrix}

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

JF=[101ω[sin(ωΔt+θ)−sin(θ)vkω[cos(ωΔt+θ)−cos(θ)vkΔtωcos(ωΔt+θ)−vkω2[sin(ωΔt+θ)−sin(θ)]011ω[−cos(ωΔt+θ)+cos(θ)vkω[sin(ωΔt+θ)−sin(θ)vkΔtωsin(ωΔt+θ)−vkω2[−cos(ωΔt+θ)+cos(θ)]001000001000001]J_F=\begin{bmatrix}1&0&\frac{1}{\omega}[sin(\omega \Delta t+\theta)-sin(\theta)&\frac{v_k}{\omega}[cos(\omega \Delta t+\theta)-cos(\theta)&\frac{v_k\Delta t}{\omega}cos(\omega \Delta t+\theta)-\frac{v_k}{\omega^2}[sin(\omega \Delta t+\theta)-sin(\theta)]\\0&1&\frac{1}{\omega}[-cos(\omega \Delta t+\theta)+cos(\theta)&\frac{v_k}{\omega}[sin(\omega \Delta t+\theta)-sin(\theta)&\frac{v_k\Delta t}{\omega}sin(\omega \Delta t+\theta)-\frac{v_k}{\omega^2}[-cos(\omega \Delta t+\theta)+cos(\theta)]\\0&0&1&0&0\\0&0&0&1&0\\0&0&0&0&1\end{bmatrix}

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

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

zk=[ρkθkρ˙k]=[xk2+yk2arctan(ykxk)vxxk+vyykxk2+yk2]=[xk2+yk2arctan(ykxk)vxxkcos(θ)+vyyksin(θ)xk2+yk2]z_k=\begin{bmatrix}\rho_k\\\theta_k\\\dot{\rho}_k\end{bmatrix}=\begin{bmatrix}\sqrt{x_k^2+y_k^2}\\arctan(\frac{y_k}{x_k})\\\frac{v_xx_k+v_yy_k}{\sqrt{x_k^2+y_k^2}}\end{bmatrix}=\begin{bmatrix}\sqrt{x_k^2+y_k^2}\\arctan(\frac{y_k}{x_k})\\\frac{v_xx_kcos(\theta)+v_yy_ksin(\theta)}{\sqrt{x_k^2+y_k^2}}\end{bmatrix}

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

JH=[xx2+y2yx2+y2000−yx2+y2xx2+y2000y(vcos(θ)y−vsin(θ)x)(x2+y2)32x(vsin(θ)x−vcos(θ)y)(x2+y2)32xcos(θ)+ysin(θ)x2+y2vcos(θ)y−vsin(θ)xx2+y20]J_H=\begin{bmatrix}\frac{x}{\sqrt{x^2+y^2}}&\frac{y}{\sqrt{x^2+y^2}}&0&0&0\\-\frac{y}{x^2+y^2}&\frac{x}{x^2+y^2}&0&0&0\\\frac{y(vcos(\theta)y-vsin(\theta)x)}{(x^2+y^2)^{\frac{3}{2}}}&\frac{x(vsin(\theta)x-vcos(\theta)y)}{(x^2+y^2)^{\frac{3}{2}}}&\frac{xcos(\theta)+ysin(\theta)}{\sqrt{x^2+y^2}}&\frac{vcos(\theta)y-vsin(\theta)x}{\sqrt{x^2+y^2}}&0\end{bmatrix}

Q矩阵计算

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

aa∼N(0,σa2)aω∼N(0,σω2)a_a\sim N(0,\sigma_a^2)\\a_\omega\sim N(0,\sigma_\omega^2)

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

w=[Δt2cos(θ)aa2Δt2sin(θ)aa2ΔtaaΔt2aω2Δtaω]=[Δt2cos(θ)20Δt2sin(θ)20Δt00Δt220Δt][aaaω]=Guw=\begin{bmatrix}\frac{\Delta t^2cos(\theta)a_a}{2}\\\frac{\Delta t^2sin(\theta)a_a}{2}\\\Delta t a_a\\\frac{\Delta t^2a_\omega}{2}\\\Delta t a_\omega\end{bmatrix}=\begin{bmatrix}\frac{\Delta t^2cos(\theta)}{2}&0\\\frac{\Delta t^2sin(\theta)}{2}&0\\\Delta t&0\\0&\frac{\Delta t^2}{2}\\0&\Delta t \end{bmatrix}\begin{bmatrix}a_a\\a_\omega \end{bmatrix}=Gu

则

Q=cov(w)=E(wwT)=GE(uuT)GT=G[σa200σω2]GTQ=cov(w)=E(ww^T)=GE(uu^T)G^T=G\begin{bmatrix}\sigma_a^2&0\\0&\sigma_\omega^2 \end{bmatrix}G^T

带入之后得

Q=[T4σa2cos2(θ)4T4σa2cos(θ)sin(θ)4T3σa2cos(θ)200T4σa2sin(θ)cos(θ)4T4σa2sin2(θ)4T3σa2sin(θ)200T3σa2cos(θ)2T3σa2sin(θ)2T2σa200000T4σa24T3σω22000T3σω22T2σω2]Q=\begin{bmatrix}\frac{T^4\sigma_a^2cos^2(\theta)}{4}&\frac{T^4\sigma_a^2cos(\theta)sin(\theta)}{4}&\frac{T^3\sigma_a^2cos(\theta)}{2}&0&0\\\frac{T^4\sigma_a^2sin(\theta)cos(\theta)}{4}&\frac{T^4\sigma_a^2sin^2(\theta)}{4}&\frac{T^3\sigma_a^2sin(\theta)}{2}&0&0\\\frac{T^3\sigma_a^2cos(\theta)}{2}&\frac{T^3\sigma_a^2sin(\theta)}{2}&T^2\sigma_a^2&0&0\\0&0&0&\frac{T^4\sigma_a^2}{4}&\frac{T^3\sigma_\omega^2}{2}\\0&0&0&\frac{T^3\sigma_\omega^2}{2}&T^2\sigma_\omega^2\end{bmatrix}

常用的CV模型的状态量为

X=[xyvxvy]X=\begin{bmatrix}x\\y\\v_x\\v_y\end{bmatrix}

其状态方程为

Xk+1=Xk+[vxΔtvyΔt00]=[xk+vxΔtyk+vyΔtvxvy]X_{k+1}=X_k+\begin{bmatrix}v_x\Delta t\\v_y\Delta t\\0\\0\end{bmatrix}=\begin{bmatrix}x_k+v_x\Delta t\\y_k+v_y\Delta t\\v_x\\v_y\end{bmatrix}

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

JF=[10Δt0010Δt00100001]J_F=\begin{bmatrix}1&0&\Delta t&0\\0&1&0&\Delta t\\0&0&1&0\\0&0&0&1\end{bmatrix}

如果假设观测量为

zk=[ρkθkρ˙k]=[xk2+yk2arctan(ykxk)vxxk+vyykxk2+yk2]z_k=\begin{bmatrix}\rho_k\\\theta_k\\\dot{\rho}_k\end{bmatrix}=\begin{bmatrix}\sqrt{x_k^2+y_k^2}\\arctan(\frac{y_k}{x_k})\\\frac{v_xx_k+v_yy_k}{\sqrt{x_k^2+y_k^2}}\end{bmatrix}

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

JH=[xkxk2+yk2ykxk2+yk200−ykxk2+yK2xkxk2+yk200yk(vxyk−vyxk)(xk2+yk2)32xk(vyxk−vxyk)(xk2+yk2)32xkxk2+yk2ykxk2+yk2]J_H=\begin{bmatrix}\frac{x_k}{\sqrt{x_k^2+y_k^2}}&\frac{y_k}{\sqrt{x_k^2+y_k^2}}&0&0\\-\frac{y_k}{x_k^2+y_K^2}&\frac{x_k}{x_k^2+y_k^2}&0&0\\\frac{y_k(v_xy_k-v_yx_k)}{(x_k^2+y_k^2)^{\frac{3}{2}}}&\frac{x_k(v_yx_k-v_xy_k)}{(x_k^2+y_k^2)^{\frac{3}{2}}}&\frac{x_k}{\sqrt{x_k^2+y_k^2}}&\frac{y_k}{\sqrt{x_k^2+y_k^2}}\end{bmatrix}

Q矩阵计算

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

ax∼N(0,σa2)ay∼N(0,σω2)a_x\sim N(0,\sigma_a^2)\\a_y\sim N(0,\sigma_\omega^2)

w=[Δt2ax2Δt2ay2ΔtaxΔtay]=[Δt2200Δt22Δt00Δt][axay]=Guw=\begin{bmatrix}\frac{\Delta t^2a_x}{2}\\\frac{\Delta t^2a_y}{2}\\\Delta t a_x\\\Delta t a_y\end{bmatrix}=\begin{bmatrix}\frac{\Delta t^2}{2}&0\\0&\frac{\Delta t^2}{2}\\\Delta t&0\\0&\Delta t \end{bmatrix}\begin{bmatrix}a_x\\a_y\end{bmatrix}=Gu

则

Q=cov(w)=E(wwT)=GE(uuT)GT=G[σx200σy2]GTQ=cov(w)=E(ww^T)=GE(uu^T)G^T=G\begin{bmatrix}\sigma_x^2&0\\0&\sigma_y^2 \end{bmatrix}G^T

Q=[T4σx240T3σx2200T4σy240T3σy22T3σx220T2σx200T3σy220T2σy2]Q=\begin{bmatrix}\frac{T^4\sigma_x^2}{4}&0&\frac{T^3\sigma_x^2}{2}&0\\0&\frac{T^4\sigma_y^2}{4}&0&\frac{T^3\sigma_y^2}{2}\\\frac{T^3\sigma_x^2}{2}&0&T^2\sigma_x^2&0\\0&\frac{T^3\sigma_y^2}{2}&0&T^2\sigma_y^2\end{bmatrix}

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

其中的系统状态量为

X=[xyθvωa]X=\begin{bmatrix}x\\y\\\theta\\v\\\omega\\a\end{bmatrix}

系统状态方程为

Xk+1=Xk+[∫tktk+1v(t)cos(θ(t))dt∫tktk+1v(t)sin(θ(t))dtωΔtaΔt00]X_{k+1}=X_k+\begin{bmatrix}\int_{t_k}^{t_{k+1}}{v(t)cos(\theta(t))}dt\\\int_{t_k}^{t_{k+1}}{v(t)sin(\theta(t))}dt\\\omega \Delta t\\a\Delta t\\0\\0\end{bmatrix}

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

∫tktk+1v(t)cos(θ(t))dt=∫tktk+1(vk+a(t−tk))cos(θk+ω(t−tk))dt=1ω2[(vkω+aωΔt)sin(θk+ωΔt)+acos(θk+ωΔt)−vkωsin(θk)−acos(θk)]∫tktk+1v(t)sin(θ(t))dt=∫tktk+1(vk+a(t−tk))sin(θk+ω(t−tk))dt=1ω2[−(vkω+aωΔt)cos(θk+ωΔt)−asin(θk+ωΔt)+vkωcos(θk)+asin(θk)]\int_{t_k}^{t_{k+1}}{v(t)cos(\theta(t))}dt=\int_{t_k}^{t_{k+1}}{(v_k+a(t-t_k))cos(\theta_k+\omega(t-t_k))}dt=\frac{1}{\omega^2}[(v_k\omega+a\omega\Delta t)sin(\theta_k+\omega \Delta t)+acos(\theta_k+\omega\Delta t)-v_k\omega sin(\theta_k)-acos(\theta_k)]\\\int_{t_k}^{t_{k+1}}{v(t)sin(\theta(t))}dt=\int_{t_k}^{t_{k+1}}{(v_k+a(t-t_k))sin(\theta_k+\omega(t-t_k))}dt=\frac{1}{\omega^2}[-(v_k\omega+a\omega\Delta t)cos(\theta_k+\omega \Delta t)-asin(\theta_k+\omega\Delta t)+v_k\omega cos(\theta_k)+asin(\theta_k)]

所以上式最终可以表示为

Xk+1=[xk+1ω2[(vkω+aωΔt)sin(θk+ωΔt)+acos(θk+ωΔt)−vkωsin(θk)−acos(θk)]y+1ω2[−(vkω+aωΔt)cos(θk+ωΔt)−asin(θk+ωΔt)+vkωcos(θk)+asin(θk)]θk+ωΔtvk+aΔtωa]X_{k+1}=\begin{bmatrix}x_k+\frac{1}{\omega^2}[(v_k\omega+a\omega\Delta t)sin(\theta_k+\omega \Delta t)+acos(\theta_k+\omega\Delta t)-v_k\omega sin(\theta_k)-acos(\theta_k)]\\y+\frac{1}{\omega^2}[-(v_k\omega+a\omega\Delta t)cos(\theta_k+\omega \Delta t)-asin(\theta_k+\omega\Delta t)+v_k\omega cos(\theta_k)+asin(\theta_k)]\\\theta_k+\omega \Delta t\\v_k+a\Delta t\\\omega\\a\end{bmatrix}

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

JF=[101ω2[(vω+aωT)cos(θ+ωT)−asin(θ+ωT)−vωcos(θ)+asin(θ)]1ω2[ωsin(θ+ωT)−ωsin(θ)]−Tasin(Tω+θ)+T(Tωa+ωv)cos(Tω+θ)−vsin(θ)+(Ta+v)sin(Tω+θ)ω2−2(−ωvsin(θ)−acos(θ)+acos(Tω+a)+(Tωa+ωv)sin(Tω+θ)ω3Tωsin(Tω+θ)−cos(θ)+cos(Tω+θ)ω2011ω2[(vω+aωT)sin(θ+ωT)+acos(θ+ωT)+vωsin(θ)−acos(θ)]1ω2[−ωcos(θ+ωT)+ωcos(θ)]Tacos(Tω+θ)+T(Tωa+ωv)sin(Tω+θ)+vcos(θ)−(Ta+v)cos(Tω+θ)ω2−2(ωvcos(θ)−asin(θ)+asin(Tω+a)−(Tωa+ωv)cos(Tω+θ)ω3−Tωcos(Tω+θ)−sin(θ)+sin(Tω+θ)ω20010T000010T000010000001]J_F=\begin{bmatrix}1&0&\frac{1}{\omega^2}[(v\omega+a\omega T)cos(\theta+\omega T)-asin(\theta+\omega T)-v\omega cos(\theta)+asin(\theta)]&\frac{1}{\omega^2}[\omega sin(\theta+\omega T)-\omega sin(\theta)]&\frac{-Tasin(T\omega+\theta)+T(T\omega a+\omega v)cos(T\omega +\theta)-vsin(\theta)+(Ta+v)sin(T\omega+\theta)}{\omega^2}-\frac{2(-\omega vsin(\theta)-acos(\theta)+acos(T\omega+a)+(T\omega a+\omega v)sin(T\omega +\theta)}{\omega^3}&\frac{T\omega sin(T\omega +\theta)-cos(\theta)+cos(T\omega +\theta)}{\omega^2}\\0&1&\frac{1}{\omega^2}[(v\omega+a\omega T)sin(\theta+\omega T)+acos(\theta+\omega T)+v\omega sin(\theta)-acos(\theta)]&\frac{1}{\omega^2}[-\omega cos(\theta+\omega T)+\omega cos(\theta)]&\frac{Tacos(T\omega+\theta)+T(T\omega a+\omega v)sin(T\omega +\theta)+vcos(\theta)-(Ta+v)cos(T\omega+\theta)}{\omega^2}-\frac{2(\omega vcos(\theta)-asin(\theta)+asin(T\omega+a)-(T\omega a+\omega v)cos(T\omega +\theta)}{\omega^3}&\frac{-T\omega cos(T\omega +\theta)-sin(\theta)+sin(T\omega +\theta)}{\omega^2}\\0&0&1&0&T&0\\0&0&0&1&0&T\\0&0&0&0&1&0\\0&0&0&0&0&1\end{bmatrix}

无迹卡尔曼滤波 UKF

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

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

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

使用背景

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

核心思想

v2-008fc966f40ba5c834c555c2dd91bac1_720w.webp

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

UKF的原理与实现流程

  1. UT变换

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

    y‾=∑i=0Lωm(i)yiPyy=∑i=0Lωc(i)(yi−y‾)(yi−y‾)TPxy=∑i=0Lωc(i)(xi−x‾)(yi−y‾)T\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({xi,ωm(i),ωc(i)},L,Px(x))=0g(\{x_i,\omega_m^{(i)},\omega_c^{(i)}\},L,P_x(x))=0

    其中

    • {xi,ωm(i),ωc(i)}\{x_i,\omega_m^{(i)},\omega_c^{(i)}\} 为 Sigma 采样点，均值权重和方差权重组合的集合
    • L 为采样点个数
    • Px(x)P_x(x) 为随机变量 x 的密度函数
    • g()g() 确定 x 的相关信息。如果密度函数 Px(x)P_x(x) 只有一二阶矩时可以写成

    g({xi,ωm(i),ωc(i)},L,Px(x))=[∑i=0Lωm(i)−1∑i=0Lωc(i)−1∑i=0Lwm(i)xi−x‾∑i=0Lωc(i)(xi−x‾)(xi−x‾)T−Pxx]=0g(\{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 的均值和方差分别为 x‾\overline{x} 和 PxP_x，假设 L=2n，则 Sigma 采样点共有 2n+1 个，对应的采样点和均值权值和方差权值

    X(i)={X‾i=0X‾+((n+κ)P)ii=1∼nX‾−((n+κ)P)ii=n+1∼2nωm(i)=ωc(i)={κn+κi=0κ2(n+κ)i=1∼2nX^{(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 的均值 x‾\overline{x} 和 Sigma 采样点间距离的比例因子，其取值的大小直接反应二阶以上的高阶矩引起的偏差
    • ((n+κ)P)i(\sqrt{(n+\kappa) P})_i 定义为矩阵 (n+κ)P(n+\kappa) P 经过 CholeskyCholesky 分解求得的平方根矩阵的第 i 行或列

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

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

    • 比例修正算法

      xi′=x0+α(xi−x0)(ωm(i))′={ωm(i)α2+1−1α2i=0ωm(i)α2i≠0(ωc(i))′={(ωm(i))′+(1+β−α2)i=0(ωm(i))′i≠0x_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≤α≤10\leq\alpha\leq 1，控制 α\alpha 的值可以控制 Sigma 点集的范围，通常设置为一个很小的正数。 β\beta 反映状态历史信息的高阶特性，对于高斯分布来说 β=2\beta=2 为最优

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

      (X(i))′={X‾i=0X‾+((n+λ)P)ii=1∼nX‾−((n+λ)P)ii=n+1∼2n(ωm(i))′={λn+λi=0λ2(n+λ)i=1∼2n(ωc(i))′={λn+λ+(1−α2+β)i=0λ2(n+λ)i=1∼2n(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.

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

  3. 状态方程与观测方程

    Xk+1=f(Xk)+wkZk=h(Xk)+vkX_{k+1}=f(X_k)+w_k\\Z_k=h(X_k)+v_k

    其中，

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

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

    X(i)={X‾i=0X‾+((n+λ)P)ii=1∼nX‾−((n+λ)P)ii=n+1∼2nX^{(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.

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

    对上式进行整理

    Xk∣k(i)=[X^k∣kX^k∣k+((n+λ)Pk∣k)iX^k∣k−((n+λ)Pk∣k)i]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步预测

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

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

    设计计算权重值

    ωm(i)={λn+λi=0λ2(n+λ)i=1∼2nωc(i)={λn+λ+(1−α2+β)i=0λ2(n+λ)i=1∼2n\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 表示协方差，上标为几个采样点，其中

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

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

    X^k+1∣k=∑i=02nωm(i)Xk+1∣k(i)Pk+1∣k=∑i=02nωc(i)[Xk+1∣k(i)−X^k+1∣k][Xk+1∣k(i)−X^k+1∣k]T+Q\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. 根据上式的预测均值 X^k+1∣k\hat{X}_{k+1|k} 和协方差矩阵 Pk+1∣kP_{k+1|k}，再次使用 UT 变换，产生新的 2n+1 个 Sigma点集

    Xk+1∣k(i)=[X^k+1∣k(X^k+1∣k+(n+λ)Pk+1∣k)iX^k+1∣k−((n+λ)Pk+1∣k)i]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 步预测观测量

    Zk+1∣k(i)=h(Xk+1∣k(i))Z_{k+1|k}^{(i)}=h(X_{k+1|k}^{(i)})

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

    Z‾k+1∣k=∑i=02nωm(i)Zk+1∣k(i)Pzkzk=∑i=02nωc(i)[Zk+1∣k(i)−Z‾k+1∣k][Zk+1∣k(i)−Z‾k+1∣k]T+RPxkzk=∑i=02nωc(i)[Xk+1∣k(i)−X^k+1∣k][Zk+1∣k(i)−Z‾k+1∣k]T\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增益矩阵

    Kk+1=PxkzkPzkzk−1K_{k+1}=P_{x_kz_k}P_{z_kz_k}^{-1}

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

    X^k+1∣k+1=X^k+1∣k+Kk+1[Zk+1−Z‾k+1∣k]Pk+1∣k+1=Pk+1∣k−Kk+1PzkzkKk+1T\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}

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

  12. 最终小结

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