INS 量测更新
5 量测更新
5.1 GNSS位置及速度更新
r ^ G P S , i n = r ^ I M U n + D R − 1 C b n l b v ^ G P S , i n = v ^ I M U n + ω i n n × C b n l b − C b n ω i b b × l b \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} r^GPS,inv^GPS,in=r^IMUn+DR−1Cbnlb=v^IMUn+ωinn×Cbnlb−Cbnωibb×lb
其中
v ^ G P S , i n = v ^ I M U n − C b n ω n b b × l b = v ^ I M U n − C b n ( ω n i b + ω i b b ) × l b = v ^ I M U n + C b n ω i n b × l b − C b n ω i b b × l b = v ^ I M U n + C b n ω i n b × C n b C b n l b − C b n ω i b b × l b = v ^ I M U n + ω i n n × C b n l b − C b n ω i b b × l b \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} v^GPS,in=v^IMUn−Cbnωnbb×lb=v^IMUn−Cbn(ωnib+ωibb)×lb=v^IMUn+Cbnωinb×lb−Cbnωibb×lb=v^IMUn+Cbnωinb×CnbCbnlb−Cbnωibb×lb=v^IMUn+ωinn×Cbnlb−Cbnωibb×lb根据上式可以导出观测方程，首先导出位置的观测方程：
r ^ G P S , i n = r ^ I M U n + D R − 1 C ^ b n l b r ^ G P S , i n = r I M U n + δ r I M U n + D R − 1 ( I − ϕ × ) l n r ^ G P S , i n − r I M U n + D R − 1 l n = δ r I M U n + D R − 1 l n × ϕ \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} r^GPS,inr^GPS,inr^GPS,in−rIMUn+DR−1ln=r^IMUn+DR−1C^bnlb=rIMUn+δrIMUn+DR−1(I−ϕ×)ln=δrIMUn+DR−1ln×ϕ然后再导出速度的观测方程：
v ^ G P S , i n = v ^ I M U n + ω i n n × C ^ b n l b − C ^ b n ω i b b × l b v ^ G P S , i n = v I M U n + δ v I M U n + ω i n n × ( I + ϕ × ) C b n l b − ( I + ϕ × ) C b n ω i b b × l b v ^ G P S , i n = v I M U n + δ v I M U n + ω i n n × l n + ω i n n × ϕ × l n − C b n ω i b b × l b − ϕ × C b n ω i b b × l b v ^ G P S , i n = v I M U n + δ v I M U n + ω i n n × l n − ω i n n × l n × ϕ − C b n ω i b b × l b + C b n ω i b b × l b × ϕ v ^ G P S , i n − v I M U n − ω i n n × l n + C n b ω i b b × l b = δ v I M U n − ω i n n × l n × ϕ + C b n ω i b b × l b × ϕ \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} v^GPS,inv^GPS,inv^GPS,inv^GPS,inv^GPS,in=v^IMUn+ωinn×C^bnlb−C^bnωibb×lb=vIMUn+δvIMUn+ωinn×(I+ϕ×)Cbnlb−(I+ϕ×)Cbnωibb×lb=vIMUn+δvIMUn+ωinn×ln+ωinn×ϕ×ln−Cbnωibb×lb−ϕ×Cbnωibb×lb=vIMUn+δvIMUn+ωinn×ln−ωinn×ln×ϕ−Cbnωibb×lb+Cbnωibb×lb×ϕ−vIMUn−ωinn×ln+Cnbωibb×lb=δvIMUn−ωinn×ln×ϕ+Cbnωibb×lb×ϕ