公式表示无损卡尔曼滤波

无损卡尔曼滤波（Unscented Kalman Filter，UKF）是一种基于卡尔曼滤波的估计方法，用于处理非线性系统。它通过引入一组“sigma点”，在非线性函数附近进行采样和传播，以近似真实的后验分布。其公式表示如下：

1. 预测步骤：

- 根据上一时刻的状态估计值 $x_{k-1}$ 和协方差矩阵 $P_{k-1}$，计算 $2n+1$ 个 sigma 点：
$$
\begin{aligned}
& X_{k-1}^{[0]} = x_{k-1} \\
& X_{k-1}^{[i]} = x_{k-1} + (\sqrt{(n+\lambda)P_{k-1}})_i, \quad i=1,\ldots,n \\
& X_{k-1}^{[i+n]} = x_{k-1} - (\sqrt{(n+\lambda)P_{k-1}})_i, \quad i=1,\ldots,n
\end{aligned}
$$
其中，$\lambda$ 是一个可调参数，通常取值为 $3-n$。
- 将这些 sigma 点通过系统动力学方程进行时间更新：
$$
\hat{X}_k^{[i]} = f(X_{k-1}^{[i]}) + w_{k-1}^{[i]}, \quad i=0,\ldots,2n
$$
其中，$w_{k-1}^{[i]}$ 是一个高斯噪声，满足 $w_{k-1}^{[i]} \sim N(0,Q_{k-1})$。
- 计算预测状态估计值 $\hat{x}_k^-$ 和协方差矩阵 $P_k^-$：
$$
\begin{aligned}
& \hat{x}_k^- = \sum_{i=0}^{2n} w_i^{[m]} \hat{X}_k^{[i]} \\
& P_k^- = \sum_{i=0}^{2n} w_i^{[c]} (\hat{X}_k^{[i]} - \hat{x}_k^-)(\hat{X}_k^{[i]} - \hat{x}_k^-)^T + Q_k
\end{aligned}
$$
其中，$w_i^{[m]}$ 和 $w_i^{[c]}$ 是权重系数，通常取值为：
$$
\begin{aligned}
& w_i^{[m]} = \begin{cases}
\frac{\lambda}{n+\lambda}, & i=0 \\
\frac{1}{2(n+\lambda)}, & i=1,\ldots,2n
\end{cases} \\
& w_i^{[c]} = \begin{cases}
\frac{\lambda}{n+\lambda} + (1-\alpha^2+\beta), & i=0 \\
\frac{1}{2(n+\lambda)}, & i=1,\ldots,2n
\end{cases}
\end{aligned}
$$
其中，$\alpha$ 和 $\beta$ 是两个可调参数，通常取值为 $\alpha=1e-3$，$\beta=2$。

2. 更新步骤：

- 根据测量模型 $h(\cdot)$，将预测状态估计值 $\hat{x}_k^-$ 映射到测量空间，得到 $2n+1$ 个测量 sigma 点：
$$
\hat{Z}_k^{[i]} = h(\hat{X}_k^{[i]}) + v_k^{[i]}, \quad i=0,\ldots,2n
$$
其中，$v_k^{[i]}$ 是一个高斯噪声，满足 $v_k^{[i]} \sim N(0,R_k)$。
- 计算预测测量值 $\hat{z}_k^-$ 和协方差矩阵 $P_{xz,k}^-$：
$$
\begin{aligned}
& \hat{z}_k^- = \sum_{i=0}^{2n} w_i^{[m]} \hat{Z}_k^{[i]} \\
& P_{xz,k}^- = \sum_{i=0}^{2n} w_i^{[c]} (\hat{X}_k^{[i]} - \hat{x}_k^-)(\hat{Z}_k^{[i]} - \hat{z}_k^-)^T
\end{aligned}
$$
- 计算卡尔曼增益 $K_k$ 和更新后的状态估计值 $x_k$ 和协方差矩阵 $P_k$：
$$
\begin{aligned}
& K_k = P_{xz,k}^- (P_k^-)^{-1} \\
& x_k = \hat{x}_k^- + K_k (z_k - \hat{z}_k^-) \\
& P_k = P_k^- - K_k P_{xz,k}^-^T
\end{aligned}
$$
其中，$z_k$ 是测量值。