## 3 Onboard Train Localization Algorithm
### OD Measurement Modeling  
The OD installation and the relative positions of the OD and SINS are shown in Figure 4. The projected component of the OD speed measurement in the OD coordinate system is \(\widetilde{\mathbf{v}}_{D}^{m}=\left[0\quad\widetilde{v}_{D}\quad 0\right]^{ \text{T}}\). The OD speed under the \(n\) system is expressed as \(\widetilde{\mathbf{v}}_{D}^{n}\). \(\widetilde{\mathbf{C}}_{m}^{b}\) is the direction cosine matrix (DCM) from m frame to b frame; \(\widetilde{\mathbf{C}}_{b}^{n}\) is the DCM from b system to n system. \(\alpha_{\theta},\alpha_{\varphi}\) are pitch and yaw misalignment angles from the m frame to the b frame, respectively. \(\mathbf{\phi}^{n}\) is the attitude misalignment angle of SINS. \(\delta K\) and \(\mathbf{w}_{D}^{m}\) are the scale factor of the OD and the measurement noise, respectively. \(L_{D}^{b}\) is the lever arm between the SINS and OD. Because the SINS/OD solution is performed in the n coordinate system, the first step is to construct a measurement error model from the \(m\) system to the \(n\) system  
\[\left\{ \begin{array}{l}\widetilde{\mathbf{v}}_{D}^{n}=\widetilde{\mathbf{C}}_{b}^{n }\widetilde{\mathbf{c}}_{m}^{b}\widetilde{\mathbf{c}}_{D}^{m}\\ \widetilde{\mathbf{C}}_{b}^{n}=[\mathbf{I}_{3\times 3}-(\mathbf{\phi}^{n}\times)]\mathbf{C}_{b}^{n },\ \widetilde{\mathbf{C}}_{m}^{b}=[\mathbf{I}_{3\times 3}-(\delta\mathbf{\kappa}\times)]\mathbf{C}_{m}^ {b}\\ \delta\mathbf{\alpha}=\left[\begin{array}{cc}\delta\alpha_{\theta}&0&\delta \alpha_{\varphi}\end{array}\right]^{\text{T}},\ \widetilde{\mathbf{v}}_{D}^{m}=(1+\delta K_{D})\mathbf{v}_{D}^{m}+\mathbf{w}_{D}^{m} \end{array} \right. \tag{4}\]  
Expanding the first term in Equation (4) yields:  
\[ \begin{array}{l}\widetilde{\mathbf{v}}_{D}^{n}=[\mathbf{I}_{3\times 3}-(\mathbf{\phi}^{n} \times)]\mathbf{C}_{b}^{n}[\mathbf{I}_{3\times 3}-(\delta\mathbf{\kappa}\times)]\mathbf{C}_{m}^ {b}((1+\delta K_{D})\mathbf{v}_{D}^{m}+\mathbf{w}_{D}^{m})\\ =[\mathbf{I}_{3\times 3}-(\mathbf{\phi}^{n}\times)]\mathbf{C}_{b}^{n}[\mathbf{I}_{3\times 3}-( \delta\mathbf{\kappa}\times)]\mathbf{C}_{m}^{b}(\mathbf{v}_{D}^{m}+\delta K_{D}\mathbf{v}_{D}^ {m}+\mathbf{w}_{D}^{m})\\ =[\mathbf{I}_{3\times 3}-(\mathbf{\phi}^{n}\times)]\mathbf{C}_{b}^{n}[\mathbf{v}_{D}^{b}+\delta K _{D}\mathbf{v}_{D}^{b}+\mathbf{v}_{D}^{b}\times\delta\mathbf{\alpha}+\mathbf{w}_{D}^{b}]\\ \end{array}  \tag{5}\]  
In order to convert the speed measured by the OD to the SINS measurement point, the effect of the lever arm needs to be taken into account, so we have:  
\[\widetilde{\mathbf{v}}_{DI}^{b}=\mathbf{v}_{D}^{b}+\delta K_{D}\mathbf{v}_{D}^{b}+\mathbf{v}_ {D}^{b}\times\delta\mathbf{\alpha}-\mathbf{\omega}_{eb}^{b}\times\widetilde{L}_{D}^{ b}+\mathbf{w}_{D}^{b}(1) \tag{6}\]  
We take the result of Equation (6) and bring it into Equation (5), and Equation (7) is obtained as follows:  
\[\begin{array}{l}\widetilde{\mathbf{v}}_{DI}^{n}=[\mathbf{I}_{3\times 3}-(\mathbf{\phi}^{n} \times)]\mathbf{C}_{b}^{n}\Big{[}\mathbf{v}_{D}^{b}+\delta K_{D}\mathbf{v}_{D}^{b}+\mathbf{v}_ {D}^{b}\times\delta\mathbf{\alpha}-\mathbf{\omega}_{eb}^{b}\times\widetilde{L}_{D}^{ b}+\mathbf{w}_{D}^{b}\Big{]}\\ =[\mathbf{I}_{3\times 3}-(\mathbf{\phi}^{n}\times)]\mathbf{C}_{b}^{n}\Big{[}\mathbf{v}_{D}^{b}-\mathbf{ \omega}_{eb}^{b}\times L_{D}^{b}+\delta K_{D}\mathbf{v}_{D}^{b}+\mathbf{v}_{D}^{b}\times \delta\mathbf{\alpha}-\mathbf{\omega}_{eb}^{b}\times\delta L_{D}^{b}+\mathbf{w}_{D}^{b} \Big{]}\\ =[\mathbf{I}_{3\times 3}-(\mathbf{\phi}^{n}\times)]\mathbf{C}_{b}^{n}\Big{[}\mathbf{v}_{D \parallel}^{b}+\delta K_{D}\mathbf{v}_{D}^{b}+\mathbf{v}_{D}^{b}\times\delta\mathbf{ \alpha}-\mathbf{\omega}_{eb}^{b}\times\delta L_{D}^{b}+\mathbf{w}_{D}^{b}\Big{]}\\ =[\mathbf{I}_{3\times 3}-(\mathbf{\phi}^{n}\times)]\mathbf{\left[\mathbf{v}_{DI}^{n}+\delta K_{D}\mathbf{v }_{D}^{n}+\mathbf{v}_{D}^{b}\times\delta\mathbf{\alpha}-\mathbf{\omega}_{eb}^{n}\times \delta L_{D}^{n}+\mathbf{w}_{D}^{b}\right]}\\ \approx\mathbf{v}_{DI}^{n}+\mathbf{v}_{DI}  
where  
\[M_{v}^{b}=