INS 量测更新

最新推荐文章于 2024-10-17 11:39:06 发布

xiaoooli

最新推荐文章于 2024-10-17 11:39:06 发布

阅读量791

点赞数 23

文章标签：算法机器学习人工智能

本文链接：https://blog.csdn.net/qq_40230900/article/details/135022992

版权

5 量测更新

5.1 GNSS位置及速度更新

$\begin{aligned} \hat{r}_{GPS,i}^{n} &= \hat{r}_{IMU}^{n} + D_{R}^{-1}C_{b}^{n} l^b\\ \hat{v}_{GPS,i}^{n} &= \hat{v}_{IMU}^{n} + \omega_{in}^{n}\times C_{b}^{n}l^b - C_{b}^{n}\omega_{ib}^{b}\times l^b \end{aligned}$
其中
$\begin{aligned} \hat{v}_{GPS,i}^{n} &= \hat{v}_{IMU}^{n} - C_{b}^{n}\omega_{nb}^{b}\times l^b\\ &= \hat{v}_{IMU}^{n} - C_{b}^{n}(\omega_{ni}^{b} + \omega_{ib}^{b})\times l^b\\ &= \hat{v}_{IMU}^{n} + C_{b}^{n}\omega_{in}^{b}\times l^b - C_{b}^{n}\omega_{ib}^{b}\times l^b\\ &= \hat{v}_{IMU}^{n} + C_{b}^{n}\omega_{in}^{b}\times C_{n}^{b}C_{b}^{n}l^b - C_{b}^{n}\omega_{ib}^{b}\times l^b\\ &= \hat{v}_{IMU}^{n} + \omega_{in}^{n}\times C_{b}^{n}l^b - C_{b}^{n}\omega_{ib}^{b}\times l^b\\ \end{aligned}$ 根据上式可以导出观测方程，首先导出位置的观测方程：
$\begin{aligned} \hat{r}_{GPS,i}^{n} &= \hat{r}_{IMU}^{n} + D_{R}^{-1}\hat{C}_{b}^{n} l^b\\ \hat{r}_{GPS,i}^{n} &= r_{IMU}^{n} + \delta r_{IMU}^{n} + D_{R}^{-1}(I-\phi\times)l^n\\ \hat{r}_{GPS,i}^{n} - r_{IMU}^{n} + D_{R}^{-1}l^n &= \delta r_{IMU}^{n} + D_{R}^{-1}l^n\times\phi \\ \end{aligned}$ 然后再导出速度的观测方程：
$\begin{aligned} \hat{v}_{GPS,i}^{n} &= \hat{v}_{IMU}^{n} + \omega_{in}^{n}\times \hat{C}_{b}^{n}l^b - \hat{C}_{b}^{n}\omega_{ib}^{b}\times l^b \\ \hat{v}_{GPS,i}^{n} &= v_{IMU}^{n} +\delta v_{IMU}^{n} + \omega_{in}^{n}\times (I+\phi\times) C_{b}^{n}l^b - (I+\phi\times)C_{b}^{n}\omega_{ib}^{b}\times l^b \\ \hat{v}_{GPS,i}^{n} &= v_{IMU}^{n} +\delta v_{IMU}^{n} + \omega_{in}^{n}\times l^n +\omega_{in}^{n}\times \phi\times l^n - C_{b}^{n}\omega_{ib}^{b}\times l^b -\phi\times C_{b}^{n}\omega_{ib}^{b}\times l^b \\ \hat{v}_{GPS,i}^{n} &= v_{IMU}^{n} +\delta v_{IMU}^{n} + \omega_{in}^{n}\times l^n - \omega_{in}^{n}\times l^n\times\phi - C_{b}^{n}\omega_{ib}^{b}\times l^b + C_{b}^{n}\omega_{ib}^{b}\times l^b\times\phi \\ \hat{v}_{GPS,i}^{n} &- v_{IMU}^{n} - \omega_{in}^{n}\times l^n + C_{n}^{b}\omega_{ib}^{b}\times l^b = \delta v_{IMU}^{n} - \omega_{in}^{n}\times l^n\times\phi + C_{b}^{n}\omega_{ib}^{b}\times l^b\times\phi \\ \end{aligned}$