UKF原理与实现
卡尔曼滤波家族主要有KF、EKF、UKF三兄弟。KF主要应用于系统方程和量测方程为线性的的场景，EKF应用于轻度非线性场景，而UKF则在强非线性情况下有更好的表现。这里只介绍UKF。
在KF中，我们需要求取状态协方差来确定卡尔曼增益，但是由于系统存在非线性，只能得到它的近似值。EKF通过雅可比矩阵对系统进行线性化，而UKF则是利用随机采样的条件均值代替均值，使用被估计量和量测量的再现样本点计算两者的自协方差阵和互协方差阵(
U
T
UT
UT变换）。
UT变换
对于一维状态变量的情况，可以将这个过程看作是选取 μ x , μ x + σ x , μ x − σ x \mu_x,\mu_x+\sigma_x,\mu_x-\sigma_x μx,μx+σx,μx−σx三个点，用非线性变换 g g g投影过去，再拟合出一个高斯分布。
U
T
UT
UT变换的步骤如下：
假设
n
n
n维随机向量
X
X
X经
f
(
⋅
)
f(·)
f(⋅)非线性变换后形成
m
m
m维随机向量
Y
Y
Y,即
Y
=
f
(
X
)
Y=f(X)
Y=f(X),在已知
X
X
X的均值
X
ˉ
\bar{X}
Xˉ和方差阵
P
X
X
P_{XX}
PXX的情况下,求
Y
Y
Y的均值
Y
ˉ
\bar{Y}
Yˉ和
P
Y
Y
P_{YY}
PYY
步骤1：
根据
X
ˉ
\bar{X}
Xˉ和
P
X
X
P_{XX}
PXX增广出
2
n
+
1
2n+1
2n+1个
X
X
X的1倍
σ
\sigma
σ样本点。
χ
(
0
)
=
X
ˉ
\chi^{(0)}=\bar{X}
χ(0)=Xˉ
χ
(
i
)
=
X
ˉ
+
(
(
n
+
λ
)
P
X
X
)
(
i
)
i
=
1
,
2
,
.
.
.
,
n
\chi^{(i)}=\bar{X}+(\sqrt{(n+\lambda) P_{XX}})_{(i)} \qquad i=1,2,...,n
χ(i)=Xˉ+((n+λ)PXX)(i)i=1,2,...,n
χ
(
i
)
=
X
ˉ
−
(
(
n
+
λ
)
P
X
X
)
(
i
−
n
)
i
=
n
+
1
,
n
+
2
,
.
.
.
,
2
n
\chi^{(i)}=\bar{X}-(\sqrt{(n+\lambda) P_{XX}})_{(i-n)} \qquad i=n+1,n+2,...,2n
χ(i)=Xˉ−((n+λ)PXX)(i−n)i=n+1,n+2,...,2n
式中，
(
(
n
+
λ
)
P
X
X
)
(
i
)
(\sqrt{(n+\lambda) P_{XX}})_{(i)}
((n+λ)PXX)(i)表示矩阵
(
n
+
λ
)
P
X
X
(n+\lambda)P_{XX}
(n+λ)PXX的下三角分解平方根的第
i
i
i列，为
n
n
n维列向量。
步骤2：
计算非线性变换产生的样本点：
Y
(
i
)
=
f
[
χ
(
i
)
]
Y^{(i)}=f[\chi^{(i)}]
Y(i)=f[χ(i)]
步骤3：
确定权值
W
0
(
m
)
=
λ
n
+
λ
W_0^{(m)}=\frac{\lambda}{n+\lambda}
W0(m)=n+λλ
W
0
(
c
)
=
λ
n
+
λ
+
1
−
α
2
+
β
W_0^{(c)}=\frac{\lambda}{n+\lambda}+1-\alpha^2+\beta
W0(c)=n+λλ+1−α2+β
W
i
(
m
)
=
W
i
(
c
)
=
λ
2
(
n
+
λ
)
i
=
1
,
2
,
.
.
.
,
2
n
W_i^{(m)}=W_i^{(c)}=\frac{\lambda}{2(n+\lambda)} \qquad i=1,2,...,2n
Wi(m)=Wi(c)=2(n+λ)λi=1,2,...,2n
上式中，
λ
=
α
2
(
n
+
κ
)
−
n
\lambda = \alpha^2(n+\kappa)-n
λ=α2(n+κ)−n
α
\alpha
α是很小的正数，可取
1
0
−
4
≤
α
≤
1
10^{-4}\leq \alpha \leq 1
10−4≤α≤1;
κ
=
3
−
n
\kappa=3-n
κ=3−n;
β
\beta
β取值与
X
X
X的分布有关，对于正太分布，
β
=
2
\beta=2
β=2为最优值。
步骤4：
确定映射的均值和方差阵
Y
ˉ
≈
Σ
i
=
0
2
n
W
i
(
m
)
Y
(
i
)
\bar{Y}\approx \Sigma_{i=0}^{2n}W_i^{(m)}Y^{(i)}
Yˉ≈Σi=02nWi(m)Y(i)
P
Y
Y
≈
Σ
i
=
0
2
n
W
i
(
c
)
[
Y
(
i
)
−
Y
ˉ
]
[
Y
(
i
)
−
Y
ˉ
]
T
P_{YY} \approx \Sigma_{i=0}^{2n} W_i^{(c)}[Y^{(i)}-\bar{Y}][Y^{(i)}-\bar{Y}]^T
PYY≈Σi=02nWi(c)[Y(i)−Yˉ][Y(i)−Yˉ]T
UKF过程
步骤1，选定滤波初值：
X
^
0
=
E
X
0
\hat{X}_0=EX_0
X^0=EX0
P
0
=
E
[
(
X
0
−
X
^
0
)
(
X
0
−
X
^
0
)
T
]
P_0=E[(X_0-\hat{X}_0)(X_0-\hat{X}_0)^T]
P0=E[(X0−X^0)(X0−X^0)T]
步骤2，计算
k
−
1
k-1
k−1时刻的
2
n
+
1
2n+1
2n+1个
σ
\sigma
σ样本点
χ
(
0
)
=
X
ˉ
\chi^{(0)}=\bar{X}
χ(0)=Xˉ
χ
(
i
)
=
X
ˉ
+
γ
(
P
k
−
1
)
(
i
)
i
=
1
,
2
,
.
.
.
,
n
\chi^{(i)}=\bar{X}+\gamma(\sqrt{P_{k-1}})_{(i)} \qquad i=1,2,...,n
χ(i)=Xˉ+γ(Pk−1)(i)i=1,2,...,n
χ
(
i
)
=
X
ˉ
−
γ
(
P
k
−
1
)
(
i
−
n
)
i
=
n
+
1
,
n
+
2
,
.
.
.
,
2
n
\chi^{(i)}=\bar{X}-\gamma(\sqrt{P_{k-1}})_{(i-n)} \qquad i=n+1,n+2,...,2n
χ(i)=Xˉ−γ(Pk−1)(i−n)i=n+1,n+2,...,2n
上式中，
γ
=
n
+
λ
\gamma = \sqrt{n+\lambda}
γ=n+λ
C++实现：
https://github.com/realjc/fusion-ukf
参考链接：
https://zhuanlan.zhihu.com/p/35729804
https://www.cnblogs.com/gaoxiang12/p/5560360.html
https://drivingc.com/p/5b46b7af2392ec1a513a8773
卡尔曼滤波与导航原理P228 秦永元等