# Automatic Estimation of Dynamic Lever Arms for a Position and Orientation System
## 3 Proposed SINS/GPS Integrated Navigation Model
### Error Distribution Analysis
For ease of depiction, the discretized state filtering loop is listed here:  
\[\hat{x}_{k}=\Phi_{k,k-1}\hat{x}_{k-1}, \tag{21}\]  
\[\hat{x}_{k}=\hat{x}_{k/k-1}+\overset{\wedge}{K}_{k}\left(\hat{z}_{k}-\overset{ \wedge}{H}_{k}\hat{x}_{k/k-1}\right), \tag{22}\]  
while the covariance calculation loop is updated as follows:  
\[\overset{\wedge}{P}_{k/k-1}=\overset{\wedge}{\Phi}_{k,k-1}\overset{\wedge}{P} _{k-1}\overset{\wedge}{\Phi}_{k,k-1}+\overset{\wedge}{\Gamma}_{k-1}\overset{ \wedge}{\Omega}_{k-1}\overset{\wedge}{\Gamma}_{k-1}, \tag{23}\]  
\[\overset{\wedge}{K}_{k}=\overset{\wedge}{P}_{k/k-1}\overset{\wedge}{\Pi}_{k} \overset{\wedge}{(H}_{k}\overset{\wedge}{P}_{k/k-1}\overset{\wedge}{H}_{k}+ \overset{\wedge}{R}_{k})^{-1}, \tag{24}\]  
\[\overset{\wedge}{P}_{k}=\left(\mathbf{I}-\overset{\wedge}{K}_{k}\overset{ \wedge}{H}_{k}\right)\overset{\wedge}{P}_{k/k-1}\left(\mathbf{I}-\overset{ \wedge}{K}_{k}\overset{\wedge}{H}_{k}\right)^{\mathrm{T}}+\overset{\wedge}{K }_{k}\overset{\wedge}{R}_{k}\overset{\wedge}{K}_{k}, \tag{25}\]  
where the hat label \"\({}^{\sim}\)\" denotes the calculated value in the reduced-order filter model, distinguishing it from that of the full-order filter model; the subscript \(k-1\) and \(k\) denote the last and current cycle, while \(k/k-1\) expresses the propagation value; \(\overset{\wedge}{\Phi}_{k,k-1}\), \(\overset{\wedge}{\Gamma}_{k-1}\) and \(\overset{\wedge}{H}_{k}\) are discretized values from the state equation and measurement equation; \(\overset{\wedge}{\Omega}_{k-1}\) and \(\overset{\wedge}{R}_{k}\) are the previously set covariance values of process noises and measurement noises; \(\overset{\wedge}{P}_{k}\) is the current covariance matrix recursively calculated from the initial value \(\overset{\wedge}{P}_{0}\); and \(\overset{\wedge}{K}_{k}\) is the calculated filtering gain matrix. It can be seen that the covariance calculation loop is independent of the state filtering loop. Once the filter is designed and the trajectory is selected, the associated matrices \(\overset{\wedge}{\Phi}_{k,k-1}\), \(\overset{\wedge}{P}_{0}\), \(\overset{\wedge}{Q}_{k-1}\), \(\overset{\wedge}{R}_{k}\) and \(\overset{\wedge}{H}_{k}\) are uniquely determined, so is the gain matrix \(\overset{\wedge}{K}_{k}\). The filtering program only needs to be run once, so that the gain matrix \(\overset{\wedge}{K}_{k}\) can be obtained at every measurement update. It should be recorded and substituted into the full-order filter model for covariance analysis. In the full-order filter model, the state equation and measurement equation are extended as follows:  
\[x_{k}^{*}=\mathbf{\phi}_{k/k-1}^{*}x_{k-1}^{*}+\mathbf{\Gamma}_{k-1}^{*}\mathbf{W}_{k-1}^ {*} \tag{26}\]  
\[z_{k}^{*}=\mathbf{H}_{k}^{*}x_{k}^{*}+\mathbf{V}_{k}^{*} \tag{27}\]  
where the superscript \"\({}^{\sim}\)\" denotes the matrix involved in the full-order filter model. It can be seen that there are three types of errors that affect the estimation results: the initial state errors \(\mathbf{x}_{0}^{*}\), the process noises \(\mathbf{W}_{k-1}^{*}\), and the measurement noises \(\mathbf{V}_{k}^{*}\). Based on the linear error model, the estimated error of a concerned state at any moment can be expressed as a linear cumulative sum:  
\[x_{i}^{*}=\sum_{j=1}^{n}S_{i,j}^{*}x_{j0}^{*}+\sum_{j=1}^{l}x_{i,W_{j}}^{*}+ \sum_{j=1}^{m}x_{i,V_{j}}^{*}, \tag{28}\]  
where \(\mathbf{S}^{*}\) is the propagation matrix of initial errors; \(n\), \(l\), and \(m\) are the nonzero element numbers of the initial state, process noise, and measurement noise, respectively; \(x_{i,W_{j}}^{*}\) or \(x_{i,V_{j}}^{*}\) denotes the estimated error of \(i\)th state caused by the \(j\)th process noise \(W_{j}\) or the \(j\)th measurement noise \(V_{j}\). For the sake of simplicity, the current cycle \(k\) is omitted in Equation (28). The filtering gain matrix \(\overset{\wedge}{\mathbf{K}}_{k}\) is the crucial link between the reduced-order filter model and the full-order filter model, which can be extended into the full-order filter model as follows:  
\[\mathbf{K}_{k}^{*}=\left[