# Automatic Estimation of Dynamic Lever Arms for a Position and Orientation System
## 3 Proposed SINS/GPS Integrated Navigation Model  
### SINS/GPS Integrated Navigation Model  
As shown in Equation (11), if the SINS/GPS integrated navigation model can be designed to estimate the lever arms \(\mathbf{L}_{OG}^{b_{c}}\) and \(\mathbf{L}_{OI}^{b}\), the complex dynamic lever arm \(\mathbf{L}_{IG}^{b_{c}}\) can be compensated automatically, and the tedious work of manual measurement can be eliminated. Combining the classical error model of SINS and the constant lever arms \(\mathbf{L}_{OI}^{b}\) and \(\mathbf{L}_{OG}^{b_{c}}\), we define a 21-dimensional error state vector of SINS/GPS integration as:  
\[\mathbf{x}(t)=\Big{[}\ \ (\mathbf{\phi}^{n})^{\mathrm{T}}\ \ \ (\delta\mathbf{v}^{n})^{ \mathrm{T}}\ \ \ (\delta\mathbf{p}_{I})^{\mathrm{T}}\ \ \ (\varepsilon^{b})^{\mathrm{T}}\ \ \ (\
abla^{b})^{\mathrm{T}}\ \ \ (\mathbf{L}_{OI}^{b})^{ \mathrm{T}}\ \ (\mathbf{L}_{OG}^{b_{c}})^{\mathrm{T}}\ \Big{]}^{\mathrm{T}}, \tag{13}\]  
where \(\mathbf{\phi}^{n}\!\!=\![\phi_{E},\phi_{N},\phi_{U}]^{\mathrm{T}}\) is the misalignment angles of SINS; \(\delta\mathbf{v}^{n}\!\!=\![\delta v_{E},\delta v_{N},\delta v_{U}]^{\mathrm{T}}\) is the velocity errors; \(\delta\mathbf{p}_{I}=[\delta L,\delta\lambda,\delta h]^{\mathrm{T}}\) denotes the position errors consisting of latitude, longitude, and height errors; \(\varepsilon^{b}\) and \(\
abla^{b}\) are the gyro and accelerometer biases in the \(b\)-frame, respectively.  
The corresponding state equation of Kalman filter is:  
\[\dot{\mathbf{x}}(t)=\left[\begin{array}{cc}\mathbf{F}_{I}&0_{15\times 6}\\ 0_{6\times 15}&0_{6\times 6}\end{array}\right]\mathbf{x}(t)+\left[\begin{array}{c} -\mathbf{C}_{b}^{n}\mathbf{\varepsilon}_{w}^{b}\\ \mathbf{C}_{b}^{n}\
abla_{w}^{b}\\ 0_{15\times 1}\end{array}\right], \tag{14}\]  
where \(\mathbf{F}_{I}\) is a 15 \(\times\) 15 transition matrix based on typical SINS error model as detailed in [25]. The symbols \(\mathbf{\varepsilon}_{w}^{b}\) and \(\
abla_{w}^{b}\) are the noises of gyros and accelerometers, and \(0_{i\times j}\) denotes an \(i\times j\) zero matrix.  
For loosely coupled SINS/GPS integrated navigation, there are two measurement models to be selected in practical POS, including position measurements and position/velocity measurements. The associated measurement equations are:  
\[\left\{\begin{array}{l}\mathbf{z}_{p}=\mathbf{p}_{I}-\mathbf{p}_{G}=\mathbf{H}_{p}\mathbf{x}(t) +\mathbf{w}_{gp}\\ \mathbf{z}_{v}=\mathbf{v}_{I}^{n}-\mathbf{v}_{G}^{n}=\mathbf{H}_{p}\mathbf{x}(t)+\mathbf{w}_{gv}\end{array} \right., \tag{15}\]  
where \(\mathbf{w}_{gp}\) and \(\mathbf{w}_{gv}\) represent the position and velocity noises of the GPS.  
Since Equation (11) can be rewritten as:  
\[\left\{\begin{array}{l}\mathbf{p}_{I}-\mathbf{p}_{G}=\mathbf{M}_{p}\mathbf{C}_{b}^{n}\mathbf{L} _{OI}^{b}-\mathbf{M}_{p}\mathbf{C}_{b_{c}}^{n}\mathbf{L}_{OG}^{b_{c}}\\ \mathbf{v}_{I}^{n}-\mathbf{v}_{G}^{n}=\mathbf{C}_{b}^{n}\left(\mathbf{\omega}_{eb}^{b}\times\mathbf{ L}_{OI}^{b}\right)-\mathbf{C}_{b_{c}}^{n}\left(\mathbf{\omega}_{eb_{c}}^{b_{c}} \times\mathbf{L}_{OG}^{b_{c}}\right)\end{array}\right., \tag{16}\]the measurement matrices \(\mathbf{H}_{p}\) and \(\mathbf{H}_{v}\) in Equation (15) are obtained as:  
\[\left\{\begin{array}{l}\mathbf{H}_{p}=\left[\begin{array}{cccccccc}0_{3\times 3}&0_{3 \times 3}&\mathbf{I}_{3}&0_{3\times 3}&0_{3\times 3}&M_{p}\mathbf{C}_{b}^{n}-M_{p}\mathbf{C}_{ b}^{n}\mathbf{C}_{b_{c}}^{b}\end{array}\right]\\ \mathbf{H}_{v}=\left[\begin{array}{cccccccc}0_{3\times 3}&\mathbf{I}_{3\times 3}&0_{3 \times 3}&0_{3\times 3}&0_{3\times 3}&\mathbf{C}_{b}^{n}\left(\mathbf{\omega}_{eb}^{b} \times\right)-\mathbf{C}_{b_{c}}^{n}\left(\mathbf{\omega}_{eb_{c}}^{b_{c}}\times \right)\end{array}\right]\end{array}\right.. \tag{17}\]  
In this paper, we depict the position measurement equation as an example, while the position/velocity measurement equations have the same results.