Accurate integrated position and orientation method for vehicles based on strapdown inertial navigation system /Doppler radar

Abstract

In order to realize maneuver combat in the modern warfare, some special military vehicles require the ability of determining their position and orientation rapidly and accurately, and the position and orientation system should be highly autonomous and have strong anti-jamming capability. So a high-accuracy independent position and orientation method for vehicles that utilizes strapdown inertial navigation system/Doppler radar is presented in this article. Laser gyroscopes in strapdown inertial navigation system and Doppler radar are adopted to develop a dead-reckoning system for vehicles. Subsequently, the attitude, velocity,and position-updating algorithms of dead-reckoning system are designed. The error sources of dead-reckoning system are analyzed to establish the system error model, including the attitude error equations of the mathematical platform, velocity error equations, and position error equations. The errors of strapdown inertial navigation system and deadreckoning system are selected as system states of the integrated position and orientation method. The difference between the attitude output of strapdown inertial navigation system and that of dead-reckoning system, and the difference between the position output of strapdown inertial navigation system and that of dead-reckoning system are chosen as the measurements of integrated position and orientation. Then, Kalman filter is adopted to design the filtering algorithm of integrated position and orientation. In the end, the integrated position and orientation method is validated by simulation experiment and vehicular experiment. The experimental results show that strapdown inertial navigation system/Doppler radar integration can realize accurate positioning and orientation for a long time, and the accuracy of attitude/position integration mode is significantly higher than that of velocity/position integration mode. Therefore, the former integration mode is more suitable for accurate position and orientation for vehicles.

Introduction

Some special military vehicles, such as command vehicles, launcher vehicles, and armored vehicles, require the ability of determining their position and orientation rapidly and accurately, so as to realize maneuver combat in the modern warfare.^1,2 The position and orientation system should be highly autonomous and have strong anti-jamming capability in a combat environment.

The inertial navigation system (INS), which is a known type of autonomous navigation system with strong anti-jamming ability, is well suited for military purposes. However, the navigation errors of the pure INS accumulate over time. Therefore, INS cannot achieve high-accuracy position and orientation for extended periods of time. Thus far, integrated navigation equipment, such as INS/satellite,^3–7 INS/odometer,^8–10 INS/vision ¹¹ integrated navigation systems, has been used to assist INS to achieve high-accuracy position and orientation, as presented in many papers. The INS/satellite integrated navigation system has excellent characteristics, such as high accuracy, low cost, strong maneuverability, and other features. ¹² However, the satellite signals can be easily jammed or shielded in wartime,^13,14 and the signal transmission may also fail between tall buildings or in tunnels, which would lead to the failure of positioning. Therefore, this means that INS/satellite integrated system has weak anti-jamming ability and poor autonomy, both of which limit its application in the military field. In contrast, INS/odometer integrated system offers the advantages of strong autonomy, good anti-jamming capability, and high accuracy. This integration approach is widely used in land weapon vehicles since it can provide relatively high-accuracy navigation information as well as remain autonomous. However, vehicle slipping or gliding is known to affect the odometer reading, which will introduce large measurement errors,^15,16 and reduce the accuracy of position and orientation. In order to reduce INS error accumulation, vision-aided INS with point and vertical line observations for land vehicle applications were developed. ¹¹ This method can effectively improve the accuracy of the attitude and position estimation and has the advantages of strong autonomy and anti-jamming capability.

Continuous positioning was achieved by developing INS/odometer/satellite integration navigation method based on particle filtering (PF), ¹⁷ and the performance of this method was examined using a land vehicle on road test. The test results demonstrated that this method achieved superior performance during global positioning system (GPS) becoming invalid and improved the position accuracy. Thus, INS/odometer/satellite integrated system has been considered one of the most attractive methodologies for ground vehicle navigation. However, once satellite navigation fails for long time, the accuracy of this approach will deteriorate gradually with time. To overcome this limitation, a novel ground vehicle navigation system based on an integrated INS, GPS, odometer, and vision data was proposed. ¹⁴ Experiments demonstrated that this integrated navigation approach improved the position accuracy by over 30% during simulated GPS outages.

Vehicular Doppler radar, which is based on the Doppler effect, is used to measure the vehicle velocity by calculating the difference between the frequencies of the transmitted and echo waves. Vehicular Doppler radar offers advantages such as high accuracy, absence of velocity-accumulated errors, continuous output, strong anti-jamming ability, and strong independence.^18,19 In particular, Doppler radar could avoid the measurement errors caused by vehicle slipping or gliding. A pair of Doppler radars was used to estimate the running paths of vehicle accurately. ²⁰ Since the drift errors of INS are inevitable and grow over time, Doppler velocity log (DVL) is used to aid INS to restrain its error growth. Therefore, INS/DVL integration is a good approach for autonomous underwater vehicle (AUV) navigation^21–24 and is currently being used underwater. Considering the aforementioned findings, this paper adopts Doppler radar and laser strapdown inertial navigation system (SINS) to develop an integrated position and orientation system for vehicles, for which an accurate integrated position and orientation algorithm was researched. The advantage of this integrated system is its ability to achieve rapid accurate position and orientation on land, with strong autonomy and good anti-jamming ability.

This paper describes the basic principles of this accurate integrated position and orientation method. Section “SINS/Doppler radar integrated position and orientation scheme” describes SINS/Doppler radar integrated position and orientation scheme and presents the schematic diagram. The laser gyroscope/Doppler radar dead-reckoning algorithm is provided in section “Dead-reckoning algorithm based on laser gyroscope/Doppler radar,” which is based on the angle increment output of the laser gyroscope and the velocity output of the Doppler radar, section “Error models of the laser gyroscopes/Doppler radar DRS” analyses the error sources of dead-reckoning to establish the error models of dead-reckoning system (DRS), including the attitude error equations of the mathematical platform, velocity error equations, and position error equations. Section “Filtering algorithm of the integrated position and orientation based on SINS/Doppler radar” presents the filtering algorithm of SINS/Doppler radar integrated position and orientation system, including the state equations and measurement equations for integrated position and orientation. Then, Kalman filter is adopted to design the filtering algorithm of the integrated position and orientation system in section “Filtering algorithm of the integrated position and orientation based on SINS/Doppler radar.” The experimental results are presented and analyzed in section “Experimental results,” and conclusions are drawn in section “Conclusion.”

SINS/Doppler radar integrated position and orientation scheme

The east–north–up frame is used as the navigation frame (n frame), and the right–front–up frame is considered as the vehicle body frame (b frame). Both the laser SINS and Doppler radar are installed on the vehicle. After navigation calculations, the laser SINS outputs the attitude, position, and velocity information of the vehicle, and the Doppler radar outputs the velocity of the vehicle in real time.

During the position and orientation process, laser gyroscopes in SINS and Doppler radar are used to construct the laser gyroscope/Doppler radar DRS, and the attitude and position information of the vehicle can be calculated. The difference between the attitude output of SINS and that of DRS, and the difference between the position output of SINS and that of DRS are chosen as the measurements of integrated position and orientation. Then, these measurements are sent to the integrated position and orientation filter. After the filtering calculation, the real-time estimates of system states (i.e. system errors) are calculated and used to correct the errors of SINS and DRS in real time. Finally, the outputs of the corrected SINS are considered as the outputs of SINS/Doppler radar integrated position and orientation system, including the attitude, velocity, and position of the vehicle. A schematic diagram of SINS/Doppler radar integrated position and orientation process is shown in Figure 1.

Figure 1.

Schematic diagram of SINS/Doppler radar integrated position and orientation.

Dead-reckoning algorithm based on laser gyroscope/Doppler radar

Attitude-updating algorithm of DRS

In order to realize uninterrupted signal sampling and reduce the effect of system noise, the angle increment output of the laser gyroscope is used to calculate the attitude. ²⁵ The equivalent rotating vector method is adopted to design the attitude-updating algorithm of DRS.

We assume that the attitude-updating period is $T$ ( $T=t_j-t_{j-1}$ ). The attitude quaternion of the vehicle at $t_j$ and $t_{j-1}$ is ${\mathit{q}}_{b(j)}^n$ and ${\mathit{q}}_{b(j-1)}^n$ , respectively. According to the quaternion multiplication rule, the updating equation of the vehicle attitude quaternion is expressed as

$${\mathit{q}}_{b(j)}^n={\mathit{q}}_{b(j-1)}^n{\mathit{q}}_{b(j)}^{b(j-1)}$$

(1)

where ${\mathit{q}}_{b(j)}^{b(j-1)}$ is the transform quaternion of the b frame from $t_{j-1}$ to $t_j$ , and ${\mathit{q}}_{b(j)}^{b(j-1)}$ can be calculated as

$${\mathit{q}}_{b(j)}^{b(j-1)}=\cos \frac{\Vert {\mathit{\eta }}_j\Vert }{2}+\frac{{\mathit{\eta }}_j}{\Vert {\mathit{\eta }}_j\Vert }\sin \frac{\Vert {\mathit{\eta }}_j\Vert }{2}$$

(2)

where ${\mathit{\eta }}_j={\displaystyle {\int }_{t_{j-1}}^{t_j}{\mathit{\omega }}_{nb}^b(t)dt}$ is the equivalent rotating vector from the vehicle body frame to the navigation frame from $t_{j-1}$ to $t_j$ .

Hence, once ${\mathit{\eta }}_j$ is calculated, the attitude quaternion of the vehicle can be updated using equations (1) and (2). Let us assume that the attitude matrix of the vehicle is ${\mathit{C}}_b^n$ . Then, the above equation on ${\mathit{\eta }}_j$ can be transformed into the following equation

$${\mathit{\eta }}_j={\displaystyle \underset{t_{j-1}}{\overset{t_j}{\int }}({\mathit{\omega }}_{ib}^b(t)-{({\mathit{C}}_b^n(t))}^T{\mathit{\omega }}_{in}^n(t))dt}$$

(3)

where ${\mathit{\omega }}_{ib}^b$ is the rotation angular rate of the vehicle expressed in the vehicle body frame, and ${\mathit{\omega }}_{in}^n$ is the rotating angular rate of the navigation frame expressed in the navigation frame.

In time period $[t_{j-1},t_j]$ , since the change of ${\mathit{\omega }}_{ib}^b$ is much more than the changes of ${\mathit{C}}_b^n$ and ${\mathit{\omega }}_{in}^n$ , the values of ${\mathit{C}}_b^n$ and ${\mathit{\omega }}_{in}^n$ can be regarded as the constant when $t_{j-1}\le t\le t_j$ , and their values can be considered as the values at the moment $t_{j-1}$ . Thus, the following equation is obtained from equation (3)

$${\mathit{\eta }}_j\approx {\displaystyle \underset{t_{j-1}}{\overset{t_j}{\int }}{\mathit{\omega }}_{ib}^b(t)dt}-{({{\mathit{C}}_b^n}_{(j-1)})}^T{{\mathit{\omega }}_{in}^n}_{(j-1)}T$$

(4)

Then

$${{\mathit{\omega }}_{in}^n}_{(j-1)}={\left[\begin{array}{ccc}-\frac{v_N}{R_M+h}& {\omega }_{ie}\cos L+\frac{v_E}{R_N+h}& {\omega }_{ie}\sin L+\frac{v_E\tan L}{R_N+h}\end{array}\right]}_{t_{j-1}}^T$$

(5)

where $v_E,v_N$ are the velocities of the vehicle in the east and north directions, respectively; $L,h$ are the latitude and altitude, respectively; $R_M$ is the principal curvature radius of the local meridian; $R_N$ is the principal curvature radius of the prime vertical, which is perpendicular to the meridian surface.

From equation (4), in order to calculate ${\mathit{\eta }}_j$ , ${\displaystyle {\int }_{t_{j-1}}^{t_j}{\mathit{\omega }}_{ib}^b(t)dt}$ is also calculated first. Hence, we assume that ${\mathit{\Phi }}_j={\displaystyle {\int }_{t_{j-1}}^{t_j}{\mathit{\omega }}_{ib}^b(t)dt}$ , and ${\mathit{\Phi }}_j$ is the equivalent rotating vector of the vehicle body frame from $t_{j-1}$ to $t_j$ .

As it is difficult to obtain the function of ${\mathit{\omega }}_{ib}^b(t)$ , the calculating of ${\mathit{\Phi }}_j$ by integration is impossible. Hence, in this study, the two-sample algorithm of the equivalent rotation vector is adopted to calculate ${\mathit{\Phi }}_j$ approximately. For a carrier with low dynamics, such as the vehicle, ${\mathit{\omega }}_{ib}^b$ will slowly change with time during the attitude-updating period. Thus, the vehicular angle velocity ${\mathit{\omega }}_{ib}^b$ can be approximately described by a first-order polynomial. Then, the two-sample algorithm of the equivalent rotation vector can be deduced as follows ²⁵

$${\mathit{\Phi }}_j=\mathrm{\Delta }{\mathit{\theta }}_{j1}+\mathrm{\Delta }{\mathit{\theta }}_{j2}+\frac{2}{3}\mathrm{\Delta }{\mathit{\theta }}_{j1}\times \mathrm{\Delta }{\mathit{\theta }}_{j2}$$

(6)

where $\mathrm{\Delta }{\mathit{\theta }}_{j1}$ and $\mathrm{\Delta }{\mathit{\theta }}_{j2}$ are the rotation angle increments of the vehicle in time periods $[t_{j-1},t_{j-1}+T/2]$ and $[t_{j-1}+T/2,t_j]$ , respectively, which are the sample values of the angle increment outputs of the gyroscopes between the two same intervals in the attitude-updating period T.

Thus, ${\mathit{\eta }}_j$ can be calculated by equations (4)–(6), and the real-time updating of attitude quaternion ${\mathit{q}}_b^n$ can be achieved by introducing ${\mathit{\eta }}_j$ in equations (1) and (2). We assume that the attitude quaternion of the current time calculated by DRS is ${\mathit{q}}_{bD}^n={[q_0,q_1,q_2,q_3]}^T$ ; then the attitude matrix of the vehicle ${\mathit{C}}_{bD}^n$ can be calculated by the following equation

$${\mathit{C}}_{bD}^n=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$$

(7)

After the attitude matrix of the vehicle is calculated, the heading and attitude angles of the vehicle can be calculated in real time.

Velocity and position-updating algorithms of DRS

After calculating the attitude matrix of the vehicle using attitude updating, the velocity and position of the vehicle can be calculated in real time using the velocity output of the Doppler radar. Thus, the velocity and position-updating processes are completed.

Let us assume that the velocity and position-updating periods coincide with the attitude-updating period and denote it as $T$ ( $T=t_j-t_{j-1}$ ). We further assume that the velocity output of the Doppler radar at moment $t_j$ is ${\mathit{V}}_j^b$ . According to the updated value of ${C_{bD}^n}_{(j)}$ at moment $t_j$ , the vehicle velocity in the navigational frame (n frame) at the current moment is expressed as

$${\mathit{V}}_j^n={{\mathit{C}}_{bD}^n}_{(j)}{\mathit{V}}_j^b={{\mathit{C}}_{bD}^n}_{(j)}{[\begin{array}{ccc}0& V_j^b& 0\end{array}]}^T$$

(8)

(Equation 8) is the velocity-updating algorithm of DRS. When the navigation frame is the east–north–up geographic frame, the latitude $L$ , the longitude $\lambda$ , and the altitude $h$ of the vehicle position satisfy the following differential equations

$$\stackrel{·}{L}=\frac{v_N}{R_M+h}$$

(9)

$$\stackrel{·}{\lambda }=\frac{v_E}{\left(R_N+h\right)\cos L}$$

(10)

$$\stackrel{·}{h}=v_U$$

(11)

We assume that the latitude, longitude, and altitude of the vehicle position at moment $t_{j-1}$ are $L_{j-1}$ , ${\lambda }_{j-1}$ , and $h_{j-1}$ , respectively, and those at moment $t_j$ are $L_j$ , ${\lambda }_j$ , and $h_j$ , respectively. According to differential equations (9)–(11), the position-updating algorithm of DRS can be expressed as

$$L_j=L_{j-1}+\frac{\mathrm{\Delta }{S}_{Nj}}{R_M+h_{j-1}}$$

(12)

$${\lambda }_j={\lambda }_{j-1}+\frac{\mathrm{\Delta }{S}_{Ej}}{(R_N+h_{j-1})\cos L_{j-1}}$$

(13)

$$h_j=h_{j-1}+\mathrm{\Delta }{S}_{Uj}$$

(14)

where ∆S_Ej, ∆S_Nj, and ∆S_Uj are the driving distance increments of the vehicle along the three axes of the navigation frame during $[t_{j-1},t_j]$ . Then, by making ${[\mathrm{\Delta }{S}_{Ej},\mathrm{\Delta }{S}_{Nj},\mathrm{\Delta }{S}_{Uj}]}^T=\mathrm{\Delta }{S}_j^n$ , according to the vehicle velocities ${\mathit{V}}_{j-1}^b$ and ${\mathit{V}}_j^b$ from Doppler radar at moments $t_{j-1}$ and $t_j$ , $S_j^n$ ∆ can be approximately calculated.

Because the vehicle velocity changes slowly in a short interval, the vehicle velocity during $[t_{j-1},t_j]$ can be approximately described as a first-order polynomial, that is

$$V^b(t)=at+b$$

(15)

According to the vehicle velocities ${\mathit{V}}_{j-1}^b$ and ${\mathit{V}}_j^b$ at moments $t_{j-1}$ and $t_j$ , and equation (15), the following equations can be easily obtained

$$a=\frac{(V_j^b-V_{j-1}^b)}{T},b=\frac{(V_{j-1}^b{t}_j-V_j^b{t}_{j-1})}{T}$$

(16)

From equation (16), the driving distance increment ∆S_j during $[t_{j-1},t_j]$ can be expressed as follows

$$\mathrm{\Delta }{S}_j={\displaystyle \underset{t_{j-1}}{\overset{t_j}{\int }}(at+b)dt}=(V_{j-1}^b+V_j^b)T/2$$

(17)

According to ${{\mathit{C}}_{bD}^n}_{(j)}$ calculated by the attitude updating, the distance increment in the vehicle body frame could be transformed into that in the navigation frame

$$\mathrm{\Delta }{\mathit{S}}_j^n={{\mathit{C}}_{bD}^n}_{(j)}\mathrm{\Delta }{\mathit{S}}_j^b={{\mathit{C}}_{bD}^n}_{(j)}{[\begin{array}{ccc}0& \mathrm{\Delta }{S}_j& 0\end{array}]}^T$$

(18)

By substituting equation (17) into equation (18), $\mathrm{\Delta }{\mathit{S}}_j^n$ can be expressed as follows

$$\mathrm{\Delta }{\mathit{S}}_j^n={{\mathit{C}}_{bD}^n}_{(j)}{[\begin{array}{ccc}0& (V_{j-1}^b+V_j^b)T/2& 0\end{array}]}^T$$

(19)

Hence, the position updating of DRS can be achieved using equations (12)–(14) and (19).

Error models of the laser gyroscopes/Doppler radar DRS

Sensor errors, initial condition errors, algorithm errors, and other errors exist in actual DRS, which lead to attitude error, velocity error, and position errors of DRS. All these errors accumulate with time. Hence, error sources of DRS should be analyzed to establish the system error models, so as to design the filtering algorithm to estimate and modify system errors in the integrated position and orientation determination. Thus, the accuracy of the integrated position and orientation system can be improved.

Error models of the laser gyroscopes and Doppler radar

In the laser gyroscope/Doppler radar DRS, in addition to the initial condition and algorithm errors, sensor errors are the main error sources. Thus, sensor errors are mainly considered in this study, which are composed of gyroscope errors and Doppler radar errors.

With regard to the gyroscope errors, only the constant drift and white noise are left after calibration and compensation; thus, the gyroscope error can be described as follows

$${\epsilon }_i={\epsilon }_{bi}+w_{gi}(i=x,y,z)$$

(20)

where ${\epsilon }_i(i=x,y,z)$ represents the errors of the gyroscopes installed along the x-, y-, and z-axes of the vehicle body frame, respectively, and ${\epsilon }_{bi}$ is the constant drift of the gyroscope, which can be expressed as

$${\stackrel{·}{\epsilon }}_{bi}=0(i=x,y,z)$$

(21)

where $w_{gi}$ is the white noise of the gyroscope drift, which obeys $E[w_{gi}(t)w_{gi}(\tau )]=q_{gi}\delta (t-\tau )$ , and $q_{gi}$ is the standard deviation of the gyroscope white noise.

The errors in the velocity measurement of the Doppler radar are mainly caused by the installation angle error and the scale factor error. ²¹ The installation angle error of the Doppler radar can be compensated effectively by regular calibration. Thus, the scale factor error is necessary to be modeled, which is often considered as a random constant or a first-order Markov process.^24,26 Thus, the first-order Markov process is adopted to describe the scale factor error of the Doppler radar in this study, that is

$$\delta \stackrel{·}{k}=-\delta k/{\tau }_k+w_k$$

(22)

where $\delta k$ is the scale factor error of the Doppler radar, ${\tau }_k$ is the correlation time, and $w_k$ is the white noise excitation.

Attitude error equations of DRS mathematical platform

The attitude matrix ${\mathit{C}}_{bD}^n$ calculated by DRS describes the transformation relationship between the vehicle body and the navigation frames. The function of this attitude matrix is the same as that of the attitude matrix of SINS, which realizes the function of the so-called mathematical platform. Hence, by referring to the method for deriving the attitude error equations of SINS mathematical platform, the attitude error equations of DRS mathematical platform can be deduced as follows

$$\begin{array}{c}{\stackrel{·}{\varphi }}_{DE}=\left({\omega }_{ie}\sin L+\frac{v_E}{R_N+h}\tan L\right){\varphi }_{DN}\\ -\left({\omega }_{ie}\cos L+\frac{v_E}{R_N+h}\right){\varphi }_{DU}-\frac{1}{R_M+h}\delta v_{DN}\\ +\frac{v_N}{{(R_M+h)}^2}\delta h_D-C_{11}{\epsilon }_{bx}-C_{12}{\epsilon }_{by}-C_{13}{\epsilon }_{bz}\\ -C_{11}{w}_{gx}-C_{12}{w}_{gy}-C_{13}{w}_{gz}\end{array}$$

(23)

$$\begin{array}{c}{\stackrel{·}{\varphi }}_{DN}=-\left({\omega }_{ie}\sin L+\frac{v_E}{R_N+h}\tan L\right){\varphi }_{DE}-\frac{v_N}{R_M+h}{\varphi }_{DU}\hfill \\ +\frac{1}{R_N+h}\delta v_{DE}-{\omega }_{ie}\sin L\delta L_D\hfill \\ -\frac{v_E}{{(R_N+h)}^2}\delta h_D-C_{21}{\epsilon }_{bx}-C_{22}{\epsilon }_{by}-C_{23}{\epsilon }_{bz}\hfill \\ -C_{21}{w}_{gx}-C_{22}{w}_{gy}-C_{23}{w}_{gz}\hfill \end{array}$$

(24)

$$\begin{array}{c}{\stackrel{·}{\varphi }}_{DU}=\left({\omega }_{ie}\cos L+\frac{v_E}{R_N+h}\right){\varphi }_{DE}+\frac{v_N}{R_M+h}{\varphi }_{DN}\\ +\frac{\tan L}{R_N+h}\delta v_{DE}+\left({\omega }_{ie}\cos L+\frac{v_E{\sec }^{2}L}{R_N+h}\right)\delta L_D\\ -\frac{v_E\tan L}{{(R_N+h)}^2}\delta h_D-C_{31}{\epsilon }_{bx}-C_{32}{\epsilon }_{by}-C_{33}{\epsilon }_{bz}\\ -C_{31}{w}_{gx}-C_{32}{w}_{gy}-C_{33}{w}_{gz}\end{array}$$

(25)

where ${\varphi }_{DE}$ , ${\varphi }_{DN}$ , and ${\varphi }_{DU}$ are the misalignment angles in the mathematical platform of DRS along the east, north, and up directions, respectively; $\delta L_D$ and $\delta h_D$ are the latitude errors and altitude errors of DRS, respectively; $\delta v_{DE}$ and $\delta v_{DN}$ are the velocity errors of DRS in the east and north directions, respectively; $C_{ij}(i,j=1,2,3)$ are the elements of the vehicle attitude matrix $C_{bD}^n$ in row i and column j.

Velocity error equation of DRS

We assume that the vehicle attitude matrix calculated by DRS is ${\widehat{C}}_{bD}^n$ ; the measured values and the real values of the vehicle velocity in the vehicle body frame are ${\widehat{\mathit{V}}}^b$ and ${\mathit{V}}^b$ , respectively. Considering the scale factor error of the Doppler radar, the measured vehicle velocity in the navigation frame is

$${\widehat{\mathit{V}}}^n={\widehat{\mathit{C}}}_{bD}^n{\widehat{\mathit{V}}}^b=(\mathit{I}-{\mathit{\varphi }}_D^n\times ){\mathit{C}}_{bD}^n(1+\mathit{\delta }k){\mathit{V}}^b$$

(26)

where

$${\mathit{\varphi }}_D^n\times =\left[\begin{array}{ccc}0& -{\mathit{\varphi }}_{DU}& {\mathit{\varphi }}_{DN}\\ {\mathit{\varphi }}_{DU}& 0& -{\mathit{\varphi }}_{DE}\\ -{\mathit{\varphi }}_{DN}& {\mathit{\varphi }}_{DE}& 0\end{array}\right]$$

Expanding (26) and ignoring the second-order small values of the errors, the following equation can be obtained

$${\widehat{\mathit{V}}}^n={\mathit{V}}^n-({\mathit{\varphi }}_D^n\times ){\mathit{C}}_{bD}^n{\mathit{V}}^b+\delta k{\mathit{C}}_{bD}^n{\mathit{V}}^b$$

(27)

We assume that the velocity error of DRS in the navigation frame is $\mathit{\delta }{\mathit{V}}_D^n$ , and $\mathit{\delta }{\mathit{V}}_D^n={\widehat{\mathit{V}}}^n-{\mathit{V}}^n$ . According to equation (27), we have

$$\mathit{\delta }{\mathit{V}}_D^n=-({\mathit{\varphi }}_D^n\times ){\mathit{V}}^n+\delta k{\mathit{V}}^n$$

(28)

We assume that $\mathit{\delta }{\mathit{V}}_D^n={[\mathit{\delta }{v}_{DE},\mathit{\delta }{v}_{DN},\mathit{\delta }{v}_{DU}]}^T$ and ${\mathit{V}}^n={[v_E,v_N,v_U]}^T$ , where $\delta v_{DE},\delta v_{DN},\delta v_{DU}$ are the velocity errors of DRS in the east, north, and up directions, respectively. By expanding (28), the velocity error equations of DRS can be obtained as follows

$$\delta v_{DE}=-v_U{\varphi }_{DN}+v_N{\varphi }_{DU}+v_E\delta k$$

(29)

$$\delta v_{DN}=v_U{\varphi }_{DE}-v_E{\varphi }_{DU}+v_N\delta k$$

(30)

$$\delta v_{DU}=-v_N{\varphi }_{DE}+v_E{\varphi }_{DN}+v_U\delta k$$

(31)

Position error equations of DRS

Let us assume that the latitude, longitude, and altitude errors of DRS are $\delta L_D$ , $\delta {\lambda }_D$ , and $\delta h_D$ , respectively. According to equations (9)–(11), the actual position output of DRS should satisfy the following equations

$$\stackrel{·}{L}+\delta {\stackrel{·}{L}}_D=\frac{v_N+\delta v_{DN}}{R_M+h+\delta h_D}$$

(32)

$$\stackrel{·}{\lambda }+\delta {\stackrel{·}{\lambda }}_D=\frac{v_E+\delta v_{DE}}{\left(R_N+h+\delta h_D\right)\cos (L+\delta L_D)}$$

(33)

$$\stackrel{·}{h}+\delta {\stackrel{·}{h}}_D=v_U+\delta v_{DU}$$

(34)

By expanding equations (32)–(34), ignoring the second-order small value of the errors, and comparing with equations (9)–(11), the following equations can be obtained

$$\delta {\stackrel{·}{L}}_D=\frac{1}{R_M+h}\delta v_{DN}-\frac{v_N}{{(R_M+h)}^2}\delta h_D$$

(35)

$$\delta {\stackrel{·}{\lambda }}_D=\frac{\sec L}{R_N+h}\delta v_{DE}+\frac{v_E\tan L\sec L}{R_N+h}\delta L_D-\frac{v_E\sec L}{{(R_N+h)}^2}\delta h_D$$

(36)

$$\delta {\stackrel{·}{h}}_D=\delta v_{DU}$$

(37)

Then, substituting equations (29)–(31) into equations (35)–(37), the position error equations of DRS can be obtained as follows

$$\begin{array}{c}\delta {\stackrel{·}{L}}_D=\frac{v_U}{R_M+h}{\varphi }_{DE}-\frac{v_E}{R_M+h}{\varphi }_{DU}-\frac{v_N}{{(R_M+h)}^2}\delta h_D\hfill \\ +\frac{v_N}{R_M+h}\delta k\hfill \end{array}$$

(38)

$$\begin{array}{c}\delta {\stackrel{·}{\lambda }}_D=-\frac{v_U\sec L}{R_N+h}{\varphi }_{DN}+\frac{v_N\sec L}{R_N+h}{\varphi }_{DU}+\frac{v_E\tan L\sec L}{R_N+h}\delta L_D\hfill \\ -\frac{v_E\sec L}{{(R_N+h)}^2}\delta h_D+\frac{v_E\sec L}{R_N+h}\delta k\hfill \end{array}$$

(39)

$$\delta {\stackrel{·}{h}}_D=-v_N{\varphi }_{DE}+v_E{\varphi }_{DN}+v_U\delta k$$

(40)

Filtering algorithm of the integrated position and orientation based on SINS/Doppler radar

The errors of laser gyroscope/Doppler radar DRS slowly diverge with time; thus, DRS cannot achieve independent high-accuracy position and orientation for a long time. Furthermore, because DRS cannot autonomously accomplish initial alignment, it cannot autonomously determine the initial attitude of the vehicle. Therefore, DRS and SINS are integrated to achieve position and orientation in this study. This method can not only effectively restrain the divergence of the system error, but also determine the initial attitude of the vehicle with the assistance of SINS initial alignment. Furthermore, the system cost can be reduced by sharing the three laser gyroscopes of SINS. Hence, investigating the filtering algorithm of integrated position and orientation based on SINS/Doppler radar is necessary.

State equation of integrated position and orientation

The indirect filtering method is adopted in the design of the integrated position and orientation filtering algorithm. In this indirect filtering method, system errors are chosen as the system states of the filter. Because the error models of SINS have been researched and presented in many publications, they are ignored here. And the error models of the laser gyroscopes/Doppler radar DRS have been discussed in section “Dead-reckoning algorithm based on laser gyroscope/Doppler radar.” According to equations (29)–(31), a linear relationship clearly exists between the velocity errors and the mathematical platform attitude error of DRS. Hence, the velocity errors are not chosen as the system states of the integrated position and orientation filter. The white noises of the gyroscopes and accelerometers are considered as system noises and not as system states.

Thus, the system states of SINS/Doppler integrated position and orientation include the following: SINS mathematical platform attitude errors ${\varphi }_E,{\varphi }_N,{\varphi }_U$ ; velocity errors $\delta v_E,\delta v_N,\delta v_U$ ; position errors $\delta L,\delta \lambda ,\delta h$ ; constant drifts of the gyroscopes ${\epsilon }_{bx},{\epsilon }_{by},{\epsilon }_{bz}$ ; constant biases of the accelerometers ${\nabla }_{bx},{\nabla }_{by},{\nabla }_{bz}$ ; DRS mathematical platform attitude errors ${\varphi }_{DE},{\varphi }_{DN},{\varphi }_{DU}$ ; position errors $\delta L_D,\delta {\lambda }_D,\delta h_D$ ; and the scale factor error of the Doppler radar $\delta k$ . Therefore, the system state vector of integrated position and orientation is expressed as

$$\begin{array}{c}\mathit{X}=[{\varphi }_E,{\varphi }_N,{\varphi }_U,\delta v_E,\delta v_N,\delta v_U,\delta L,\delta \lambda ,\delta h,{\epsilon }_{bx},{\epsilon }_{by},{\epsilon }_{bz},\\ {\nabla }_{bx},{\nabla }_{by},{\nabla }_{bz},{\varphi }_{DE},{\varphi }_{DN},{\varphi }_{DU},\delta L_D,\delta {\lambda }_D,\delta h_D,\delta k{]}^T\end{array}$$

(41)

According to the error models of SINS and DRS, and by combining these with the system state vector $\mathit{X}$ , the state equation of SINS/Doppler radar integrated position and orientation can be expressed as

$$\stackrel{·}{\mathit{X}}=\mathit{F}\mathit{X}+\mathit{G}\mathit{W}$$

(42)

where $\mathit{F}$ is the system state matrix, $\mathit{G}$ is the driving matrix of the system noise, and $\mathit{W}$ is the system white noise. Here, $\mathit{W}={[w_{gx},w_{gy},w_{gz},{\mathrm{w}}_{ax},w_{ay},w_{az},w_k]}^T$ , which satisfies $E[\mathit{W}(t)]=0$ and $E[\mathit{W}(t){\mathit{W}}^T(\tau )]=\mathit{q}\delta (t-\tau )$ . $w_{gx},w_{gy},w_{gz}$ are the white noises of the gyroscopes, $w_{ax},w_{ay},w_{az}$ are the white noises of the accelerometers, $w_k$ is the white noise excitation of the scale factor error, and $\mathit{q}$ is the variance matrix of $\mathit{W}$ .

Measurement equation of integrated position and orientation

In the current designs of the integrated position and orientation system based on SINS, the differences in the position or velocity outputs from each navigation subsystem are usually chosen as the measurements of the integrated position and orientation.^27,28 However, because there is no measurement information on the vehicle’s attitude, the observability of SINS attitude errors is usually poor. Thus, the system attitude errors cannot be effectively estimated and compensated, and the attitude accuracy inevitably affects the position accuracy. Furthermore, during the navigation process, the errors of the attitude and position gradually diverge with time. Hence, in order to improve the accuracy of the position and orientation, we need to introduce the attitude information into the measurement of the integrated position and orientation.

Since both SINS and DRS can output the attitude, velocity, and position information of the vehicle, several choices are available for the measurement of the integrated position and orientation. The attitude information is introduced into the measurement in this study, and then the attitude and position of the vehicle are chosen as the measurement of the integrated position and orientation. The experimental results of attitude/position integration mode are compared with that of velocity/position integration mode to verify the effect of introduction of the attitude information.

Measurement equation of the attitude/position integration mode

In the attitude/position integration mode, the difference between the attitude output of SINS and that of DRS, and the difference between the position output of SINS and that of DRS are chosen as the measurements. Thus, the measurement of the attitude/position integration mode is expressed as

$$\begin{array}{c}{\mathit{Z}}_1=[{\psi }_S-{\psi }_D,{\theta }_S-{\theta }_D,{\gamma }_S-{\gamma }_D,L_S-L_D,\hfill \\ {\lambda }_S-{\lambda }_D,h_S-h_D{]}^T\hfill \end{array}$$

(43)

where ${\psi }_S,{\mathit{\theta }}_S,{\gamma }_S$ are the heading, pitch, and roll angle outputs of SINS, respectively; ${\psi }_D,{\mathit{\theta }}_D,{\gamma }_D$ are the heading, pitch, and roll angles outputs of DRS, respectively; $L_S,{\lambda }_S,h_S$ are the latitude, longitude, and altitude outputs of SINS, respectively; $L_D,{\lambda }_D,h_D$ are the latitude, longitude, and altitude output of DRS, respectively.

We assume that the heading, pitch, and roll errors of SINS are $\delta \psi ,\delta \mathit{\theta },\delta \gamma$ , respectively, and those of DRS are $\delta {\psi }_D,\delta {\mathit{\theta }}_D,\delta {\gamma }_D$ . Then, the following equation can be obtained using equation (43)

$$\begin{array}{c}{\mathit{Z}}_1=[\delta \psi -\delta {\psi }_D,\delta \theta -\delta {\theta }_D,\delta \gamma -\delta {\gamma }_D,\\ \delta L-\delta L_D,\delta \lambda -\delta {\lambda }_D,\delta h-\delta h_D{]}^T\end{array}$$

(44)

For SINS, the relationship between the heading, pitch, roll errors, and the mathematical platform attitude errors satisfies the following equations

$$\delta \psi =-\frac{T_{12}{T}_{32}}{T_{12}^2+T_{22}^2}{\varphi }_E-\frac{T_{32}{T}_{22}}{T_{12}^2+T_{22}^2}{\varphi }_N+{\varphi }_U$$

(45)

$$\delta \theta =-\frac{T_{22}}{\sqrt{1-T_{32}^2}}{\varphi }_E+\frac{T_{12}}{\sqrt{1-T_{32}^2}}{\varphi }_N$$

(46)

$$\delta \gamma =\frac{T_{21}{T}_{33}-T_{23}{T}_{31}}{T_{31}^2+T_{33}^2}{\varphi }_E+\frac{T_{13}{T}_{31}-T_{11}{T}_{33}}{T_{31}^2+T_{33}^2}{\varphi }_N$$

(47)

Here, $T_{ij}(i,j=1,2,3)$ are the elements of the attitude matrix $C_b^n$ in row i and column j.

Similar to the above equations, for DRS, the relationship between the heading, pitch, roll errors, and the mathematical platform attitude errors satisfies the following equations

$$\delta {\psi }_D=-\frac{C_{12}{C}_{32}}{C_{12}^2+C_{22}^2}{\varphi }_{DE}-\frac{C_{32}{C}_{22}}{C_{12}^2+C_{22}^2}{\varphi }_{DN}+{\varphi }_{DU}$$

(48)

$$\delta {\theta }_D=-\frac{C_{22}}{\sqrt{1-C_{32}^2}}{\varphi }_{DE}+\frac{C_{12}}{\sqrt{1-C_{32}^2}}{\varphi }_{DN}$$

(49)

$$\delta {\gamma }_D=\frac{C_{21}{C}_{33}-C_{23}{C}_{31}}{C_{31}^2+C_{33}^2}{\varphi }_{DE}+\frac{C_{13}{C}_{31}-C_{11}{C}_{33}}{C_{31}^2+C_{33}^2}{\varphi }_{DN}$$

(50)

Here, $C_{ij}(i,j=1,2,3)$ are the elements of attitude matrix ${\mathit{C}}_{bD}^n$ in row i and column j.

By substituting equations (45)–(50) into equation (44) and combining with the system state vector $\mathit{X}$ , the measurement equation of the attitude/position integration mode can be obtained as follows

$${\mathit{Z}}_1={\mathit{H}}_1{\mathit{X}}_1+{\mathit{V}}_1$$

(51)

where ${\mathit{H}}_1$ is the measurement matrix, ${\mathit{V}}_1$ is the measurement white noise, and its variance matrix is $R_1$ .

Measurement equation of the velocity/position integration mode

In the velocity/position integration mode, the difference between the velocity output of SINS and that of DRS, and the difference between the position output of SINS and that of DRS are chosen as the measurements. Therefore, the measurement of the velocity/position integration mode is

$$\begin{array}{c}{\mathit{Z}}_2=[v_{SE}-v_{DE},v_{SN}-v_{DN},v_{SU}-v_{DU},\\ L_S-L_D,{\lambda }_S-{\lambda }_D,h_S-h_D{]}^T\end{array}$$

(52)

where $v_{SE},v_{SN},v_{SU}$ are the east, north, and up velocity outputs of SINS, respectively;

$v_{DE},v_{DN},v_{DU}$ are the east, north, and up velocity outputs of DRS, respectively.

From equation (52), we have

$$\begin{array}{c}{\mathit{Z}}_2=[\delta v_E-\delta v_{DE},\delta v_N-\delta v_{DN},\delta v_U-\delta v_{DU},\\ \delta L-\delta L_D,\delta \lambda -\delta {\lambda }_D,\delta h-\delta h_D{]}^T\end{array}$$

(53)

Substituting equations (29)–(31) into equation (53), the following equation can be obtained

$${\mathit{Z}}_2=\left[\begin{array}{l}\delta v_E+v_U{\varphi }_{DN}-v_N{\varphi }_{DU}-v_E\delta k\\ \delta v_N-v_U{\varphi }_{DE}+v_E{\varphi }_{DU}-v_N\delta k\\ \delta v_U+v_N{\varphi }_{DE}-v_E{\varphi }_{DN}-v_U\delta k\\ \delta L-\delta L_D\\ \delta \lambda -\delta {\lambda }_D\\ \delta h-\delta h_D\end{array}\right]$$

(54)

Thus, combined with the system state vector $\mathit{X}$ , the measurement equation of the velocity/position integration mode can be obtained as follows

$${\mathit{Z}}_2={\mathit{H}}_2{\mathit{X}}_2+{\mathit{V}}_2$$

(55)

where ${\mathit{H}}_2$ is the measurement matrix, ${\mathit{V}}_2$ is the white noise of the measurement, and ${\mathit{R}}_2$ is its variance matrix .

After the state equation and measurement equation are obtained, the Kalman filtering algorithm can be adopted to perform the filtering calculation of the integrated position and orientation. Thus, the real-time estimations of system state $\mathit{X}$ are calculated by the filtering calculation. Because system state $\mathit{X}$ are composed of SINS errors and DRS errors, the estimations of the system errors are taken to correct the errors of SINS and DRS in real time. Finally, the outputs of the corrected SINS are considered as the outputs of SINS/Doppler radar integrated position and orientation system, which include the attitude, velocity, and position of the vehicle.

Experimental results

Simulation experiment results

First, we design and simulate the motion trajectory of the vehicle. In the trajectory designing, the actual motion states of the vehicle are simulated, which include uniform, acceleration, deceleration, turning, climbing, downslope, and stopping motions, as well as other forms of motion. Suppose that the initial position of the vehicle is at 34°14.763′ North latitude, 108°54.579′ East longitude, and 380 m above sea level. And suppose that the initial velocity of the vehicle is 0 m/s, the initial heading angle is 30°, the initial pitch angle is 0°, the initial roll angle is 0° (these angles are all randomly set), the driving time of the vehicle is 3600 s, the highest velocity is 22 m/s, and the total driving distance is 50.6 km. MATLAB is used to simulate the motion trajectory of the vehicle, and the corresponding motion parameters are obtained by the simulation. The three-dimensional curve of the motion trajectory is shown in Figure 2.

Figure 2.

Motion trajectory of the vehicle.

Based on the aforementioned motion trajectory of the vehicle, SINS/Doppler radar integrated position and orientation method is simulated and verified. Suppose that the constant drift of the laser gyroscope is 0.01°/h and its random walk is $\sqrt{\mathrm{h}}$ 0.001°/, the constant bias of the accelerometer is 10^–4 g and its random walk is ${10}^{-5}g\cdot \sqrt{\mathrm{s}}$ , the standard deviation of the velocity measurement white noise in the Doppler radar is 0.1 m/s, the initial horizontal attitude error of SINS is 2′, the initial heading error is 5′, the initial velocity error is 0 m/s, the initial position error is 10 m, and the initial errors of DRS are the same as those of SINS. Based on the same motion trajectory and simulation conditions, the attitude/position integration and velocity/position integration methods are simulated, respectively, and the simulation results are shown in Figures 3 and 4. In the figures, the solid line indicates the simulation result of the attitude/position integration mode, and the dotted line indicates the simulation result of the velocity/position integration mode.

Figure 3.

Position errors of the integrated position and orientation simulation experiment.

Figure 4.

Attitude errors of the integrated position and orientation simulation experiment.

The above simulation results show that irrespective of the attitude/position or the velocity/position integration mode, the position and orientation results of SINS/Doppler radar integration are satisfactory. The errors of the position and orientation obviously do not diverge with time, and higher accuracy is achieved. These results prove that when the Doppler radar is used to assist SINS to achieve integrated position and orientation, the divergence problem of SINS errors can be effectively solved. Simultaneously, we can see that under the same motion trajectory and simulation conditions, the position and orientation accuracies of the attitude/position integration mode are higher than those of the velocity/position integration mode. When the filter reaches the stable state, the position error of the first integration mode is ±20.8 m (less than 0.05% of the total distance), the heading error is ±2.1′, and the horizontal attitude error is ±0.5′. However, the position error of the second integration mode is ±49.2 m (less than 0.1% of the total distance), the heading error is ±4.0′, and the horizontal attitude error is ±0.6′. In particular, during the navigation process, the position error of the attitude/position integration mode keeps good convergence, whereas that of the velocity/position integration mode shows a divergent trend. This simulation result shows that by introducing the attitude information into the measurement, not only the estimation effect of SINS attitude error is effectively improved (meaning that the orientation accuracy of the system is improved), but also the position accuracy of the system is improved. In addition, the errors of the integrated position and orientation can maintain good convergence for a long time.

Vehicular experiment results

Based on the laser SINS and Doppler radar existing in our research group laboratory, integrated position and orientation vehicular experiment is carried out. The vehicle and equipments used in experiment are shown in Figure 5. The constant drifts of laser gyroscopes in this laser SINS are better than 0.015°/h, and the random walks are better than $/\sqrt{\mathrm{h}}$ . The constant biases of accelerometers in this laser SINS are better than 10^–4 g, and the random walks are better than ${10}^{-5}g\cdot \sqrt{\mathrm{s}}$ . The velocity measurement accuracy of Doppler radar is better than 0.2 m/s. The vehicular experiment time is 2400 s, the maximum speed of the vehicle is about 120 km/h, and the minimum speed is about 3 km/h. In the process of vehicle movement, many kinds of maneuvers such as quick braking, rapid acceleration, and large angle turning are set up so as to verify whether the Doppler radar can effectively overcome the problems of large measurement errors caused by the slipping or gliding of the vehicle. The plane motion trajectory and altitude change curve of the vehicle are respectively shown in Figures 6 and 7.

Figure 5.

Vehicle and equipments used in the vehicular experiment.

Figure 6.

Plane motion trajectory of the vehicle.

Figure 7.

Altitude change curve of the vehicle.

The position accuracy of the integrated position and orientation system is evaluated by the high-accuracy differential GPS, and the orientation accuracy is evaluated by the double antenna GPS orientation system. Thus, the vehicular experiment results of integrated position and orientation are shown in Figures 8 and 9. The same as the front simulation experiment result curves, the solid line indicates the experimental result of the attitude/position integration mode, and the dotted line indicates the experimental result of the velocity/position integration mode.

Figure 8.

Position errors of the integrated position and orientation vehicular experiment.

Figure 9.

Attitude errors of the integrated position and orientation vehicular experiment.

According to the vehicular experiment results of the actual SINS/Doppler radar integrated position and orientation system, it can be seen that in the course of the 2400 s test, the horizontal position accuracy of attitude/position integration can reach ±27.9 m, the altitude accuracy reaches ±31.4 m, the heading accuracy reaches ±3.1′, and the horizontal attitude accuracy reaches ±0.8′. By contrast, under the velocity/position integration mode, the horizontal position accuracy reaches ±53.2 m, the altitude accuracy reaches ±54.9 m, the heading accuracy reaches ±6.7′, and the horizontal attitude accuracy reaches ±1.8′. Thus, it can be seen that SINS/Doppler radar integrated system can realize accurate positioning and orientation for vehicles for a long time, and the accuracy of attitude/position integration mode is significantly higher than the accuracy of velocity/position integration mode. Therefore, the attitude/position integration mode is more suitable for accurate position and orientation for vehicles.

At the same time, the results of the vehicular experiment show that the output signal of Doppler radar was not interrupted and was not disturbed by the external environment or the external electromagnetic signal in the course of the 2400 s test. In addition, although there are a lot of quick braking, rapid acceleration, large angle turning, and other maneuvers occurring in the course of vehicle driving, the results of integrated position and orientation have not been greatly affected. According to Figures 8 and 9, it can be seen that there are only a few instantaneous abrupt occurring in the position errors and attitude errors, and these errors quickly converge to the stable state. Especially, in the process of maneuver, the stability of attitude/position integration mode is obviously better than that of velocity/position integration mode. It is proved that besides high accuracy of velocity measuring, Doppler radar has the advantages of continuous output and good anti-jamming and can effectively avoid the measurement errors caused by the slipping or gliding of the vehicle. Therefore, Doppler radar is very suitable for assisting SINS to realize accurate positioning and orientation for vehicles.

Conclusion

In summary, a high-accuracy position and orientation method are presented in this paper, which has strong anti-jamming ability and good autonomy by adopting SINS and Doppler radar to determine the position and orientation of the vehicle. A dead-reckoning algorithm based on laser gyroscope/Doppler radar is proposed, and error models of DRS are developed. Then the filtering algorithm of SINS/Doppler radar integrated position and orientation is designed. The simulation experiment and vehicular experiment results show that the proposed position and orientation method not only solves the problem about the divergence of SINS errors, but also achieves accurate position and orientation for a long time. In particular, this method does not depend on any special maneuvering motion and can effectively avoid the measurement errors caused by the vehicle slipping or gliding. These fully prove the superiority of Doppler radar assisting SINS to realize positioning and orientation.

Consequently, the integrated position and orientation method based on SINS/Doppler radar offers the advantages of high accuracy, strong anti-jamming ability, good autonomy, high maneuverability, and excellent environmental adaptability. Therefore, this method has broad application prospects for the independent position and orientation of the vehicle.