EKF法的离散非线性系统空间方程?
时间: 2024-05-31 08:12:46 浏览: 7
EKF(Extended Kalman Filter)是一种针对非线性系统的扩展卡尔曼滤波方法。它通过将非线性系统通过泰勒级数展开为线性系统,从而将卡尔曼滤波方法扩展到非线性系统上。
设离散非线性系统的状态方程为:
$$
\mathbf{x}_{k+1} = f(\mathbf{x}_k,\mathbf{u}_k) + \mathbf{w}_k
$$
其中,$\mathbf{x}_k$表示第$k$个时刻的状态向量,$\mathbf{u}_k$表示第$k$个时刻的控制向量,$\mathbf{w}_k$表示第$k$个时刻的过程噪声。
测量方程为:
$$
\mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k
$$
其中,$\mathbf{z}_k$表示第$k$个时刻的测量向量,$\mathbf{v}_k$表示第$k$个时刻的测量噪声。
在EKF中,我们假设状态方程和测量方程都是可微分的,并且可以通过泰勒级数展开为一阶线性方程。具体来说,我们可以将状态方程和测量方程在当前时刻的状态估计值$\hat{\mathbf{x}}_k$和控制向量$\mathbf{u}_k$处展开为:
$$
\begin{aligned}
\mathbf{x}_{k+1} &\approx f(\hat{\mathbf{x}}_k,\mathbf{u}_k) + \frac{\partial f}{\partial \mathbf{x}}\bigg|_{\hat{\mathbf{x}}_k,\mathbf{u}_k}(\mathbf{x}_k - \hat{\mathbf{x}}_k)\\
\mathbf{z}_k &\approx h(\hat{\mathbf{x}}_k) + \frac{\partial h}{\partial \mathbf{x}}\bigg|_{\hat{\mathbf{x}}_k}(\mathbf{x}_k - \hat{\mathbf{x}}_k)
\end{aligned}
$$
将上式代入卡尔曼滤波的预测和更新步骤中,即可得到EKF的非线性系统空间方程。具体而言,预测步骤为:
$$
\begin{aligned}
\mathbf{\hat{x}}_{k|k-1} &= f(\mathbf{\hat{x}}_{k-1|k-1},\mathbf{u}_{k-1})\\
\mathbf{P}_{k|k-1} &= \frac{\partial f}{\partial \mathbf{x}}\bigg|_{\hat{\mathbf{x}}_{k-1|k-1},\mathbf{u}_{k-1}}\mathbf{P}_{k-1|k-1}\bigg(\frac{\partial f}{\partial \mathbf{x}}\bigg|_{\hat{\mathbf{x}}_{k-1|k-1},\mathbf{u}_{k-1}}\bigg)^T + \mathbf{Q}_k
\end{aligned}
$$
其中,$\mathbf{P}_{k|k-1}$表示预测误差协方差矩阵,$\mathbf{Q}_k$表示过程噪声协方差矩阵。
更新步骤为:
$$
\begin{aligned}
\mathbf{K}_k &= \mathbf{P}_{k|k-1}\frac{\partial h}{\partial \mathbf{x}}\bigg|_{\hat{\mathbf{x}}_{k|k-1}}^T\bigg(\frac{\partial h}{\partial \mathbf{x}}\bigg|_{\hat{\mathbf{x}}_{k|k-1}}\mathbf{P}_{k|k-1}\bigg(\frac{\partial h}{\partial \mathbf{x}}\bigg|_{\hat{\mathbf{x}}_{k|k-1}}\bigg)^T + \mathbf{R}_k\bigg)^{-1}\\
\mathbf{\hat{x}}_{k|k} &= \mathbf{\hat{x}}_{k|k-1} + \mathbf{K}_k(\mathbf{z}_k - h(\mathbf{\hat{x}}_{k|k-1}))\\
\mathbf{P}_{k|k} &= (\mathbf{I} - \mathbf{K}_k\frac{\partial h}{\partial \mathbf{x}}\bigg|_{\hat{\mathbf{x}}_{k|k-1}})\mathbf{P}_{k|k-1}
\end{aligned}
$$
其中,$\mathbf{K}_k$表示卡尔曼增益矩阵,$\mathbf{R}_k$表示测量噪声协方差矩阵。$\mathbf{\hat{x}}_{k|k}$和$\mathbf{P}_{k|k}$分别表示第$k$个时刻的状态估计和误差协方差矩阵。
相关推荐
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![pdf](https://img-home.csdnimg.cn/images/20210720083512.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)