无迹卡尔曼滤波公式推导
时间: 2023-11-22 13:04:53 浏览: 32
无迹卡尔曼滤波(Unscented Kalman Filter, UKF)是一种非线性滤波算法,它通过一种称为“无迹变换”的方法,将非线性函数的高斯分布转换为高斯分布,从而实现对非线性系统的滤波。下面是无迹卡尔曼滤波的公式推导过程:
1. 状态方程和观测方程
假设我们有一个非线性系统,其状态方程和观测方程分别为:
$$x_k=f(x_{k-1},u_{k-1})+\omega_{k-1}$$
$$z_k=h(x_k)+\nu_k$$
其中,$x_k$是系统在时刻$k$的状态,$u_{k-1}$是在时刻$k-1$的控制输入,$z_k$是在时刻$k$的观测值,$\omega_{k-1}$和$\nu_k$分别是过程噪声和观测噪声。
2. 无迹变换
无迹变换是一种将高斯分布映射到高斯分布的方法,它通过一组称为“sigma点”的采样点来逼近高斯分布。具体来说,对于一个$n$维高斯分布,我们可以通过以下公式计算出$2n+1$个sigma点:
$$X^{}=\mu$$
$$X^{[i]}=\mu+\sqrt{(n+\lambda)P}e_i$$
$$X^{[i+n]}=\mu-\sqrt{(n+\lambda)P}e_i$$
其中,$\mu$是高斯分布的均值,$P$是高斯分布的协方差矩阵,$\lambda$是一个可调参数,$e_i$是一个$n$维向量,其第$i$个元素为1,其余元素为0。
3. 预测步骤
在无迹卡尔曼滤波中,预测步骤和传统的卡尔曼滤波类似,只是在状态转移函数和协方差矩阵的计算中使用了sigma点。具体来说,预测步骤包括以下几个步骤:
(1)计算sigma点
根据上面的公式,计算出$2n+1$个sigma点。
(2)通过状态转移函数计算预测状态
将每个sigma点通过状态转移函数$f$进行变换,得到预测状态:
$$\hat{x}_k^{[i]}=f(X_k^{[i]},u_{k-1})$$
(3)计算预测均值和协方差矩阵
根据预测状态计算预测均值和协方差矩阵:
$$\hat{\mu}_k=\sum_{i=0}^{2n}w_i^{[m]}\hat{x}_k^{[i]}$$
$$\hat{P}_k=\sum_{i=0}^{2n}w_i^{[c]}(\hat{x}_k^{[i]}-\hat{\mu}_k)(\hat{x}_k^{[i]}-\hat{\mu}_k)^T+Q_k$$
其中,$w_i^{[m]}$和$w_i^{[c]}$是权重系数,$Q_k$是过程噪声的协方差矩阵。
4. 更新步骤
在无迹卡尔曼滤波中,更新步骤和传统的卡尔曼滤波类似,只是在观测函数和协方差矩阵的计算中使用了sigma点。具体来说,更新步骤包括以下几个步骤:
(1)计算sigma点
根据上面的公式,计算出$2n+1$个sigma点。
(2)通过观测函数计算预测观测值
将每个sigma点通过观测函数$h$进行变换,得到预测观测值:
$$\hat{z}_k^{[i]}=h(\hat{x}_k^{[i]})$$
(3)计算预测观测均值和协方差矩阵
根据预测观测值计算预测观测均值和协方差矩阵:
$$\hat{z}_k=\sum_{i=0}^{2n}w_i^{[m]}\hat{z}_k^{[i]}$$
$$S_k=\sum_{i=0}^{2n}w_i^{[c]}(\hat{z}_k^{[i]}-\hat{z}_k)(\hat{z}_k^{[i]}-\hat{z}_k)^T+R_k$$
其中,$R_k$是观测噪声的协方差矩阵。
(4)计算卡尔曼增益
根据预测状态和预测观测值计算卡尔曼增益:
$$T_k=\sum_{i=0}^{2n}w_i^{[c]}(\hat{x}_k^{[i]}-\hat{\mu}_k)(\hat{z}_k^{[i]}-\hat{z}_k)^T$$
$$K_k=T_kS_k^{-1}$$
(5)更新状态和协方差矩阵
根据卡尔曼增益和观测值更新状态和协方差矩阵:
$$x_k=\hat{x}_k+K_k(z_k-\hat{z}_k)$$
$$P_k=\hat{P}_k-K_kS_kK_k^T$$
5. 相关问题: