# Automatic Estimation of Dynamic Lever Arms for a Position and Orientation System
## 2 System Description
\right]+\mathbf{C}_{\mathbf{\psi}_{T}}\left[ \begin{array}{c }\dot{\theta}_{T}\\ 0\\ 0\end{array} \right]+\mathbf{C}_{\mathbf{\psi}_{T}}\mathbf{C}_{\theta_{T}}\left[ \begin{array} []{c}0\\ \dot{\gamma}_{T}\\ 0\end{array} \right], \tag{3}\]  
where \(\dot{\theta}_{T}\), \(\dot{\gamma}_{T}\), and \(\dot{\psi}_{T}\) are the angular rates of roll, pitch, and heading gimbals of the ISP, respectively, which are obtained by the time differentiation of the corresponding encoder data. According to the lever arm effect [24], if the position and velocity parameters at point \(\mathbf{O}_{I}\) are obtained from navigation solutions, and the lever arm \(\mathbf{L}_{IS}^{b}\) is known, the corresponding information at point \(\mathbf{O}_{S}\) can be calculated as follows:  
\[\mathbf{p}_{S}=\mathbf{p}_{I}+\mathbf{M}_{p}\mathbf{C}_{b}^{u}\mathbf{L}_{IS}^{b}, \tag{4}\]  
\[\mathbf{v}_{S}^{n}=\mathbf{v}_{I}^{n}+\mathbf{C}_{b}^{u}\left(\mathbf{\omega}_{eb}^{b}\times \mathbf{L}_{IS}^{b}\right), \tag{5}\]  
\[\mathbf{M}_{p}=\left[ \begin{array}{ccc}0&1/(R_{M}+h)&0\\ \sec L/(R_{N}+h)&0&0\\ 0&0&1\end{array} \right], \tag{6}\]  
where \(\mathbf{p}_{S}\) and \(\mathbf{p}_{I}\) denote the position information consisting of latitude, longitude, altitude; \(\mathbf{v}_{S}^{n}\) and \(\mathbf{v}_{I}^{n}\) are the velocity data at point \(\mathbf{O}_{S}\) and point \(\mathbf{O}_{I}\) in the \(n\)-frame; \(L\) and \(h\) are the local latitude and height; \(R_{N}\) and \(R_{M}\) denote the transverse and meridian radius of curvature. The relative angular rate \(\mathbf{\omega}_{eb}^{b}\) is obtained by \(\mathbf{\omega}_{eb}^{b}=\mathbf{\omega}_{ib}^{b}-\mathbf{C}_{n}^{b}\mathbf{\omega}_{ie}^{n}\), in which \(\mathbf{\omega}_{ib}^{b}\) can be acquired from the output of gyros, and \(\mathbf{\omega}_{ie}^{n}\) denotes earth's rotation rate vector in the \(n\)-frame. Similarly, the position and velocity parameter correlations among the point \(\mathbf{O}_{O}\), point \(\mathbf{O}_{I}\), and point \(\mathbf{O}_{G}\) are formulated as:  
\[\left\{ \begin{array}{l}\mathbf{p}_{I}=\mathbf{p}_{O}+\mathbf{M}_{p}\mathbf{C}_{b}^{n}\mathbf{L} _{OI}^{b}\\ \mathbf{v}_{I}^{n}=\mathbf{v}_{O}^{n}+\mathbf{C}_{b}^{u}\left(\mathbf{\omega}_{eb}^{b}\times \mathbf{L}_{OI}^{b}\right)\end{array} \right. \tag{7}\]  
\[\left\{ \begin{array}{l}\mathbf{p}_{G}=\mathbf{p}_{O}+\mathbf{M}_{p}\mathbf{C}_{b}^{n}\mathbf{L} _{OG}^{b}\\ \mathbf{v}_{G}^{n}=\mathbf{v}_{O}^{n}+\mathbf{C}_{b}^{n}\left(\mathbf{\omega}_{eb_{c}}^{b} \times\mathbf{L}_{OG}^{b}\right)\end{array} \right. \tag{8}\]  
\[\left\{ \begin{array}{l}\mathbf{p}_{G}=\mathbf{p}_{I}+\mathbf{M}_{p}\mathbf{C}_{b}^{n}\mathbf{L} _{IG}^{b}\\ \mathbf{v}_{G}^{n}=\mathbf{v}_{I}^{n}+\mathbf{C}_{b_{c}}^{n}\left(\mathbf{\omega}_{eb_{c}}^{b} \times\mathbf{L}_{IG}^{b_{c}}\right)\end{array} \right. \tag{9}\]  
where \(\mathbf{v}_{O}^{n}\) and \(\mathbf{v}_{G}^{n}\) denote the velocities at point \(\mathbf{O}_{O}\) and point \(\mathbf{O}_{G}\) in the \(n\)-frame, and the relative angular rate \(\mathbf{\omega}_{eb_{c}}^{b_{c}}\) can be obtained by:  
\[\mathbf{\omega}_{eb_{c}}^{b_{c}}=\mathbf{C}_{b}^{b_{c}}\left(\mathbf{\omega}_{eb}^{b}-\bm {\omega}_{b_{c}b}^{b}\right) \tag{10}\]  
In a normal integrated navigation mechanism, the velocity and/or position differences between the SINS and GPS solutions are generally taken as the measurement values. To improve the measurement accuracy, the dynamic lever arm \(\mathbf{L}_{IG}^{b_{c}}\) must be compensated precisely even if it is difficult. Based on Equations (7) and (8), the dynamic lever arm can be compensated as follows:  
\[\left\{ \begin{array}{l}\mathbf{p}_{I}-\mathbf{p}_{G}=-\mathbf{M}_{p}\mathbf{C}_{b_{c}}^{n} \left(\mathbf{L}_{OG}^{b_{c}}-\mathbf{C}_{b}^{b_{c}}\mathbf{L}_{OI}^{b}\right)\\ \mathbf{v}_{I}^{n}-\mathbf{v}_{G}^{n}=-\mathbf{C}_{b_{c}}^{u}\left[\mathbf{\omega}_{eb_{c}}^{b} \times\left(\mathbf{L}_{OG}^{b_{c}}-\mathbf{C}_{b}^{b_{c}}\mathbf{L}_{OI}^{b}\right) \right]\end{array} \right.