INS 量测更新

5 量测更新

5.1 GNSS位置及速度更新

$$\begin{array}{rl}{\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{array}$$
其中
$$\begin{array}{rl}{\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{array}$$根据上式可以导出观测方程，首先导出位置的观测方程：
$$\begin{array}{rl}{\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-\varphi \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 \varphi \end{array}$$然后再导出速度的观测方程：
$$\begin{array}{rl}{\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+\varphi \times )C_b^n{l}^b-(I+\varphi \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 \varphi \times l^n-C_b^n{\omega }_{ib}^b\times l^b-\varphi \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 \varphi -C_b^n{\omega }_{ib}^b\times l^b+C_b^n{\omega }_{ib}^b\times l^b\times \varphi \\ {\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 \varphi +C_b^n{\omega }_{ib}^b\times l^b\times \varphi \end{array}$$

5.2 轮速计

5.3 Lidar