UKF随笔与Python实现(无运动模型)

UKF介绍

预测

1. 首先得到初始sigma点，并计算权重w：
   $$\begin{array}{rl}\mathbf{\chi }& =\text{sigma-function}(\mathbf{x},\mathbf{P})\\ W^m,W^c& =\text{weight-function}(\mathtt{n},\mathtt{parameters})\end{array}$$
   $$\begin{array}{rl}\lambda & ={\alpha }^2(n+\kappa )-n\\ W_0^m& =\frac{\lambda }{n+\lambda }\\ W_0^c& =\frac{\lambda }{n+\lambda }+1-{\alpha }^2+\beta \\ W_i^m=W_i^c& =\frac{1}{2(n+\lambda )}i=\mathrm{1..2}n\end{array}$$
   $$\{\begin{array}{ll}{\mathcal{X}}_0=\mu & \\ {\mathcal{X}}_i=\mu +{\left[\sqrt{(n+\lambda )\Sigma }\right]}_i,& i=\mathrm{1...}n\\ {\mathcal{X}}_i=\mu -{\left[\sqrt{(n+\lambda )\Sigma }\right]}_{i-n}& i=(n+1)...2n\end{array}$$
   对于获取Sigma点中矩阵求根，使用便于计算的定义：$\Sigma ={\mathbf{SS}}^{\mathsf{T}}$, $\mathbf{S}$即为$\Sigma$的根，通常用scipy.linalg.cholesky()计算，返回上三角矩阵，因此选用每1行为1个Sigma点.
   代码：

# 初始化权重：
def calc_weight(n, alpha, beta, kappa):
    lambda_ = alpha ** 2 * (n + kappa) - n
    Wc = np.full(2 * n + 1, 1. / (2 * (n + lambda_)))  # np.full 可以避免循环
    Wm = np.full(2 * n + 1, 1. / (2 * (n + lambda_)))
    Wc[0] = lambda_ / (n + lambda_) + (1. - alpha ** 2 + beta)
    Wm[0] = lambda_ / (n + lambda_)
    return Wc, Wm
# 获取Sigma点：
def get_sigma_pnt(n, X, P, lambda_):
    sigmas = np.zeros((2 * n + 1, n))
    U = scipy.linalg.cholesky((n + lambda_) * P)  # sqrt
    sigmas[0] = X
    for k in range(n):
        sigmas[k + 1] = X + U[k]
        sigmas[n + k + 1] = X - U[k]

2. 计算先验预测： UT变换
   $$\begin{array}{rl}\mathcal{Y}=f(\mathbf{\chi },\Delta t)& \\ \bar{\mathbf{x}}& =\sum _{i=0}^{2n}{w}_i^m{\mathcal{Y}}_i\\ \bar{\mathbf{P}}& =\sum _{i=0}^{2n}{w}_i^c({\mathcal{Y}}_i-\bar{\mathbf{x}})({\mathcal{Y}}_i-\bar{\mathbf{x}}{)}^{\mathsf{T}}+\mathbf{Q}\end{array}$$

代码：

def ukf_predict(Wm, Wc, sigmas, Q):  # ukf的预测也就是UT变换
	# 状态预测：
    x = np.dot(Wm, sigmas)  # sigmas应该是传参前投影
	# 协方差预测
    kmax, n = sigmas.shape
    P = np.zeros((n, n))
    for k in range(kmax):
        y = sigmas[k] - x
        P += Wc[k] * np.outer(y, y)  # numpy不区分一维矩阵的转置，所以调用此函数
    P += Q
    return x, P
def sigma_project(sigmas, fx, dt):  # get的sigma点集，需要投影到的空间函数，dt
    sigmas_f = sigmas
    for i in sigmas.shape[0]:
        sigmas_f[i] = fx(sigmas[i], dt)  # 可能需要改
    return sigmas_f

KF与UKF先验预测对比：
$$\begin{array}{ll}\text{Kalman}& \text{Unscented}\\ & \mathcal{Y}=f(\mathbf{\chi })\\ \bar{\mathbf{x}}=\mathbf{Fx}& \bar{\mathbf{x}}=\sum w^m\mathcal{Y}\\ \bar{\mathbf{P}}={\mathbf{FPF}}^{\mathsf{T}}+\mathbf{Q}& \bar{\mathbf{P}}=\sum w^c(\mathcal{Y}-\bar{\mathbf{x}})(\mathcal{Y}-\bar{\mathbf{x}}{)}^{\mathsf{T}}+\mathbf{Q}\end{array}$$

更新

1. 所有卡尔曼都是在测量空间更新，所以需要将预测Sigma点投影到测量空间：$\mathcal{Z}=h(\mathcal{Y})$ (调用sigma_project函数)
2. 对于测量空间中的先验进行UT变换：(直接调用预测函数即可)
   $$\begin{array}{rl}{\mathbf{\mu }}_z,{\mathbf{P}}_z& =UT(\mathcal{Z},w_m,w_c,\mathbf{R})\\ {\mathbf{\mu }}_z& =\sum _{i=0}^{2n}{w}_i^m{\mathcal{Z}}_i\\ {\mathbf{P}}_z& =\sum _{i=0}^{2n}{w}_i^c({\mathcal{Z}}_i-{\mathbf{\mu }}_z)({\mathcal{Z}}_i-{\mathbf{\mu }}_z{)}^{\mathsf{T}}+\mathbf{R}\end{array}$$
3. 计算实际观测值与预测投影到观测空间的差值得到y(结合卡尔曼增益来更新预测):
   $$\mathbf{y}=\mathbf{z}-{\mathbf{\mu }}_z$$
4. 计算卡尔曼增益：(1. 状态与测量的协方差；2. 结合预测协方差得到增益)
   $${\mathbf{P}}_{xz}=\sum _{i=0}^{2n}{w}_i^c({\mathcal{Y}}_i-\bar{\mathbf{x}})({\mathcal{Z}}_i-{\mathbf{\mu }}_z{)}^{\mathsf{T}}$$
   卡尔曼增益：(其实就是状态置信度与测量空间置信度之比)
   $$\mathbf{K}={\mathbf{P}}_{xz}{\mathbf{P}}_z^{-1}$$
5. 最后得到后验：
   $$状态：\mathbf{x}=\bar{\mathbf{x}}+\mathbf{Ky}；方差：\mathbf{P}=\bar{\mathbf{P}}-\mathbf{K}{\mathbf{P}}_{\mathbf{z}}{\mathbf{K}}^{\mathsf{T}}$$
   代码：

def ukf_update(sigmas_f, xp, Pp, hx, z, Wm, Wc, R, dt):
    # 1 投影
    sigmas_h = sigma_project(sigmas_f, hx, dt)
    # 2 投影后的预测值进行UT变换
    zp, Pz = ut_ukf_predict(sigmas_h, Wm, Wc, R)
    # 3 求差值
    y = z - zp
    # 4 状态与测量的交叉方差 与 卡尔曼增益
    Pxz = np.zeros((X_dim, Z_dim))
    for i in range(sigmas_f.shape[0]):
        Pxz += Wc[i] * np.outer(sigmas_f[i] - xp, sigmas_h[i] - zp)
    K = np.dot(Pxz, np.linalg.inv(Pz))  # Kalman 增益
    # 5 计算后验
    x_post = xp + np.dot(K, y)
    P_post = Pp - np.dot(K, Pz).dot(K.T)
    return x_post, P_post

后验对比：
$$\begin{array}{ll}\text{Kalman Filter}& \text{Unscented Kalman Filter}\\ & \mathcal{Y}=f(\mathbf{\chi })\\ \bar{\mathbf{x}}=\mathbf{Fx}& \bar{\mathbf{x}}=\sum w^m\mathcal{Y}\\ \bar{\mathbf{P}}={\mathbf{FPF}}^{\mathsf{T}}+\mathbf{Q}& \bar{\mathbf{P}}=\sum w^c(\mathcal{Y}-\bar{\mathbf{x}})(\mathcal{Y}-\bar{\mathbf{x}}{)}^{\mathsf{T}}+\mathbf{Q}\\ & \mathcal{Z}=h(\mathcal{Y})\\ & {\mathbf{\mu }}_z=\sum w^m\mathcal{Z}\\ \mathbf{y}=\mathbf{z}-\mathbf{Hx}& \mathbf{y}=\mathbf{z}-{\mathbf{\mu }}_z\\ \mathbf{S}={\mathbf{H}\bar{\mathbf{P}}\mathbf{H}}^{\mathsf{T}}+\mathbf{R}& {\mathbf{P}}_z=\sum w^c(\mathcal{Z}-{\mathbf{\mu }}_z)(\mathcal{Z}-{\mathbf{\mu }}_z{)}^{\mathsf{T}}+\mathbf{R}\\ \mathbf{K}={\bar{\mathbf{P}}\mathbf{H}}^{\mathsf{T}}{\mathbf{S}}^{-1}& \mathbf{K}=\left[\sum w^c(\mathcal{Y}-\bar{\mathbf{x}})(\mathcal{Z}-{\mathbf{\mu }}_z{)}^{\mathsf{T}}\right]{\mathbf{P}}_z^{-1}\\ \mathbf{x}=\bar{\mathbf{x}}+\mathbf{Ky}& \mathbf{x}=\bar{\mathbf{x}}+\mathbf{Ky}\\ \mathbf{P}=(\mathbf{I}-\mathbf{KH})\bar{\mathbf{P}}& \mathbf{P}=\bar{\mathbf{P}}-\mathbf{K}{\mathbf{P}}_{\mathbf{z}}{\mathbf{K}}^{\mathsf{T}}\end{array}$$