CN103727938B - A kind of pipeline mapping inertial navigation odometer Combinated navigation method - Google Patents

Abstract

The invention belongs to a kind of air navigation aid, be specifically related to a kind of pipeline mapping inertial navigation odometer Combinated navigation method, it comprises the steps, (1) setting up computation model, (2) boat position calculates, (3) Kalman filter model, (4) reverse process, (5) index point correction.It is an advantage of the invention that, propose a kind of pipeline survey field inertial navigation odometer Combinated navigation method, the method recycles global test data, by forward and reverse associating Navigation, every error of inertial navigation system and odometer is estimated and compensated, finally combine index point positional information track is further revised, the high-precision pipeline track data of final acquisition.

Description

Inertial navigation odometer combined navigation method for pipeline surveying and mapping

Technical Field

The invention belongs to a navigation method, and particularly relates to an inertial navigation odometer combined navigation method for pipeline surveying and mapping, which can be applied to the field of petroleum and natural gas pipeline surveying and mapping, can effectively utilize detection data to accurately estimate and compensate errors of an inertial navigation system and an odometer, and greatly improves the measurement precision of a pipeline track.

Background

In order to ensure the safety of the oil and gas pipeline, the oil and gas pipeline needs to be regularly detected to obtain data such as pipeline track. Because oil and gas pipelines are generally laid below the surface of the earth, it is difficult to measure their specific locations and pipeline trajectories with efficient, high-precision positioning devices. The inertial navigation system is a relatively effective measuring device, has the characteristics of full autonomy, no restriction by external conditions and the like, but the positioning accuracy of the inertial navigation system diverges along with time, so that the method for performing combined navigation by using the odometer to assist the inertial navigation is an effective method for improving the combined navigation accuracy, while pipeline detection is usually post-processing after whole-course data is obtained, so that how to use the whole-course measurement data to obtain a high-accuracy measurement track is the key research content of pipeline detection.

An application of a combined navigation technology in an oil and gas pipeline surveying and mapping system (Yuetpacejiang and the like, China national institute of inertia technology, vol.16, No. 6, 2008, 12) provides a combined navigation technology for oil and gas pipeline surveying and mapping, wherein error estimation is completed by applying forward filtering, and the error of an initial moment is corrected by using a smoothing technology. In order to make full use of the whole-course test data, the invention provides a method for estimating and compensating the error quantities of an inertial navigation system and a milemeter by using a forward and reverse combined navigation and filtering method, which is equivalent to the method for estimating each error by using twice whole-course data, has higher error estimation precision compared with the method in the literature, and finally corrects the obtained track by combining the position information of the mark point so as to further improve the track information.

Disclosure of Invention

The invention aims to provide a method for effectively estimating and compensating errors of an inertial navigation system and a milemeter, which can carry out forward and reverse combined navigation and filtering on the whole-course detection data of a pipeline to complete the estimation and compensation of each error item so as to obtain high-precision pipeline track information.

The invention is realized in this way, a method for integrated navigation of inertial navigation odometer for pipeline surveying and mapping, which comprises the following steps,

(1) a calculation model is established, and the calculation model is established,

(2) the calculation of the position of the ship is carried out,

(3) a Kalman filter model is used to model the Kalman filter,

(4) the reverse treatment is carried out on the mixture,

(5) and (5) correcting the mark point.

The step (1) comprises the following steps,

1) error model

The combined navigation algorithm of the inertial navigation odometer adopts a velocity and position matching Kalman filtering method, the scheme adopts a 19-order navigation error model, and 19 error state variables are selected as

$X=\left[\begin{array}{ccccccccccccccccccc}{\mathrm{\δV}}_n& {\mathrm{\δV}}_u& {\mathrm{\δV}}_e& {\mathrm{\Φ}}_n& {\mathrm{\Φ}}_u& {\mathrm{\Φ}}_e& {\mathrm{\δL}}_D& {\mathrm{\δh}}_D& {\mathrm{\δ\λ}}_D& {\▿}_x& {\▿}_y& {\▿}_z& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z& {\mathrm{\Θ}}_Y& {\mathrm{\Θ}}_Z& \mathrm{\ΔSF}& \mathrm{\Δt}\end{array}\right]$

The state equation is:

$\stackrel{\·}{X}=\mathrm{AX}$

wherein

$A=\left[\begin{array}{cccccc}{A}_1& A_2& 0_{3\×3}& C_b^n& 0_{3\×3}& 0_{3\×4}\\ A_3& A_4& 0_{3\×3}& 0_{3\×3}& -C_b^n& 0_{3\×4}\\ 0_{3\×3}& A_5& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& A_6\\ & & & & & \\ & & 0_{10\×19}& & & \\ & & & & & \end{array}\right]$

$A_1=\left[\begin{array}{ccc}-V_u/R_M& -V_n/R_M& -2(V_e\tan L/R_N{+\mathrm{\ω}}_{\mathrm{ie}}\sin L)\\ 2V_n/R_M& 0& 2(V_e/R_N+{\mathrm{\ω}}_{\mathrm{ie}}\cos L)\\ V_e\tan L/R_N+2{\mathrm{\ω}}_{\mathrm{ie}}\sin L& -(V_e/R_N+2{\mathrm{\ω}}_{\mathrm{ie}}\cos L)& V_n\tan L/R_N-V_u/R_N\end{array}\right]$

$A_2=\left[\begin{array}{ccc}0& -f_e& f_u\\ f_e& 0& -f_n\\ -f_u& f_n& 0\end{array}\right],$ $A_3=\left[\begin{array}{ccc}0& 0& 1/R_N\\ 0& 0& \tan L/R_N\\ -1/R_M& 0& 0\end{array}\right]$

$A_4=\left[\begin{array}{ccc}0& -V_n/R_M& -{\mathrm{\ω}}_{\mathrm{ie}}\sin L-V_e\tan L/R_N\\ V_n/R_M& 0& {\mathrm{\ω}}_{\mathrm{ie}}\cos L+V_e/R_N\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L+V_e\tan L/R_N& -{\mathrm{\ω}}_{\mathrm{ie}}\cos L/-V_E/R_N& 0\end{array}\right]$

$A_5=\left[\begin{array}{ccc}0& -V_{\mathrm{De}}& V_{\mathrm{Du}}\\ V_{\mathrm{De}}& 0& -V_{\mathrm{Dn}}\\ -V_{\mathrm{Du}}& V_{\mathrm{Dn}}& 0\end{array}\right],$ $A_6=\left[\begin{array}{cccc}-C_b^n\left(\mathrm{1,3}\right)V_D& C_b^n\left(\mathrm{1,2}\right)V_D& C_b^n\left(\mathrm{1,1}\right)V_D& 0\\ -C_b^n\left(\mathrm{2,3}\right)V_D& C_b^n\left(\mathrm{2,2}\right)V_D& C_b^n\left(\mathrm{2,1}\right)V_D& 0\\ -C_b^n\left(\mathrm{3,3}\right)V_D& C_b^n\left(\mathrm{3,2}\right)V_D& C_b^n\left(\mathrm{3,1}\right)V_D& 0\end{array}\right]$

Wherein, V_n、V_u、V_eThe inertial navigation north, vertical and east speed errors are obtained; phi_n、Φ_u、Φ_eThe inertial navigation north, vertical and east misalignment angles are adopted;inertial navigation X, Y, Z axis accelerometer zero offset;_x、_y、_z: inertial navigation X, Y, Z axis gyro drift; l is_D、h_D、λ_DFor odometry dead reckoning latitude, altitude, longitudeDegree error; theta_Y、Θ_ZThe mounting error angle between the inertial navigation and the odometer along the inertial navigation Y and Z axes; Δ SF: odometer scale coefficient error; Δ t: is a time delay; f. of_n、f_u、f_eThe specific force in the north heaven-east direction measured by the inertial navigation system; v_n、V_u、V_eRespectively the north, vertical and east speeds of the inertial navigation system; omega_ieIs the earth rotation angular rate; l represents the latitude of the position where the inertial navigation system is located; r_N、R_MRespectively the curvature radius of the Mao-unitary ring, the curvature radius in the meridian plane, V_DIs the forward speed of the odometer; v_Dn、V_Du、V_DeRespectively representing the north, vertical and east speeds of the odometer dead reckoning;representation matrixThe elements corresponding to the 1 st row and the 1 st column of the 1 st row have the same meanings as the rest of the same-form variables;

2) equation of observation

The observation equation of the filter is divided into two parts, and when the speed is observed, the observation equation is as follows:

$\left[\begin{array}{c}{\mathrm{\δV}}_n\\ {\mathrm{\δV}}_u\\ {\mathrm{\δV}}_e\\ {\mathrm{\δL}}_D\\ {\mathrm{\δh}}_D\\ {\mathrm{\δ\λ}}_D\end{array}\right]=\left[\begin{array}{ccccccccccc}1& 0& 0& 0& V_{\mathrm{De}}& -V_{\mathrm{Du}}& & C_b^n\left(\mathrm{1,3}\right)V_D& -C_b^n(1,2)V_D& -C_b^n\left(\mathrm{1,1}\right)V_D& {\stackrel{\·}{V}}_n\\ 0& 1& 0& -V_{\mathrm{De}}& 0& V_{\mathrm{Dn}}& 0_{3\×9}& C_b^n\left(\mathrm{2,3}\right)V_D& -C_b^n\left(\mathrm{2,2}\right)V_D& -C_b^n(2,1)V_D& {\stackrel{\·}{V}}_u\\ 0& 0& 1& V_{\mathrm{Du}}& -V_{\mathrm{Dn}}& 0& & C_b^n\left(\mathrm{3,3}\right)V_D& -C_b^n\left(\mathrm{2,2}\right)V_D& -C_b^n\left(\mathrm{3,1}\right)V_D& {\stackrel{\·}{V}}_e\\ & & & & & & & & & & \\ & & & & & & 0_{3\×19}& & & & \\ & & & & & & & & & & \end{array}\right]X$

when reference point information exists, the observation equation is as follows:

$\left[\begin{array}{c}{\mathrm{\δV}}_n\\ {\mathrm{\δV}}_u\\ {\mathrm{\δV}}_e\\ {\mathrm{\δL}}_D\\ {\mathrm{\δh}}_D\\ {\mathrm{\δ\λ}}_D\end{array}\right]=\begin{array}{c}\left[\begin{array}{ccccc}& & & & \\ & & 0_{3\×19}& & \\ & & & & \\ & 1& 0& 0& \\ 0_{3\×6}& 0& 1& 0& 0_{3\×10}\\ & 0& 0& 1& \end{array}\right]X\end{array}$

respectively representing the acceleration values of the inertial navigation system geographic system used for updating the speed during navigation resolving; the other variables are defined in the same section as above;

3) equation discretization

The state transition matrix discretization formula is as follows

${\mathrm{\φ}}_{k+1,k}=I+T_n{A}_{T_n}+T_n{A}_{2T_n}+......+T_n{A}_{T_e}=I+\underset{t=T_n}{\overset{T_e}{\mathrm{\Σ}}}{T}_n{A}_t$

Wherein, T_nFor navigation period, T_n=0.005s，T_eFor the filter period, T_e=1s，A_tFor the state transition matrix at time t, phi_k+1,kFor the transition matrix after discretization, t =0 at the beginning of each filtering cycle;

the discretization formula of the noise array is as follows:

Q_k=QT_e

q is a noise matrix, and Q is a noise matrix,

the observed quantity solving formula is as follows:

$\left\{\begin{array}{c}{\mathrm{\δV}}_n=V_n-V_{\mathrm{Dn}}\\ {\mathrm{\δV}}_u=V_u-V_{\mathrm{Dn}}\\ {\mathrm{\δV}}_e=V_e-V_{\mathrm{De}}\\ {\mathrm{\δL}}_D=L_D-L_M\\ {\mathrm{\δh}}_D=h_D-h_M\\ {\mathrm{\δ\λ}}_D={\mathrm{\λ}}_D-{\mathrm{\λ}}_M\end{array}\right.$

wherein λ is_D、L_D、h_DLongitude, latitude, and altitude for odometry dead reckoning; lambda [ alpha ]_M、L_M、h_MThe velocity of the odometer, for longitude, latitude, altitude at the landmark point, is given by:

$\left[\begin{array}{c}{V}_{\mathrm{Dn}}\\ V_{\mathrm{Du}}\\ V_{\mathrm{De}}\end{array}\right]=C_b^n\left[\begin{array}{c}\mathrm{\ΔS}/\mathrm{dT}\\ 0\\ 0\end{array}\right]$

in the formula, Δ S is a displacement increment within 0.1S, and dT is 0.1S.

The step (2) comprises the following steps,

performing dead reckoning while performing inertial navigation, wherein the calculation period is the same as the navigation calculation period, and Tn =5 (ms); the dead reckoning position information is initialized and calculated with navigation, namely the initial longitude, latitude and height are obtained by utilizing the external binding value,

the displacement of the output carrier of the mileage meter in the ith sampling period is designed to be delta S_iThen, the expression in the inertial navigation carrier coordinate system b is:

$\mathrm{\Δ}{S}_i^b={\left[\begin{array}{ccc}\mathrm{\Δ}{S}_i& 0& 0\end{array}\right]}^T$

converting the attitude matrix of the inertial navigation system into a geographic coordinate system to obtain:

$\mathrm{\Δ}{S}_i^n=C_b^n\mathrm{\Δ}{S}_i^b$

to eliminate the odometry noise, the speed is smoothed in a 0.1s cycle.

The dead reckoning formula is as follows:

$\left\{\begin{array}{c}{L}_D\left(t\right)=L_D(t-T_n)+\mathrm{\Δ}{S}_{\mathrm{iN}}^n/R_M\\ {\mathrm{\λ}}_D\left(t\right)={\mathrm{\λ}}_D(t-T_n)+\left(\mathrm{\Δ}{S}_{\mathrm{iE}}^n\sec L\right)/R_N\\ h_D\left(t\right)=h_D(t-T_n)+\mathrm{\Δ}{S}_{\mathrm{iU}}^n\end{array}\right.$

respectively representing the displacement increment of the odometer geographic system in the navigation resolving period.

The step (3) comprises the following formula of Kalman filtering equation:

state one-step prediction

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}$

State estimation

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k[Z_k-H_k{\hat{X}}_{k,k-1}]$

Filter gain matrix

$K_k=P_{k,k-1}{H}_k^T{[H_k{P}_{k,k-1}{H}_k^T+R_k]}^{-1}$

One-step prediction error variance matrix

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k,k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k,k-1}^T$

Estimation error variance matrix

P_k=[I-K_kH_k]P_k,k-1

Wherein,in order to predict the value of the one-step state,estimate the matrix for the state, phi_k,k-1For a state one-step transition matrix, H_kFor measuring the matrix, Z_kMeasurement of quantitative value, K_kFor filtering the gain matrix, R_kFor observing noise arrays, P_k,k-1For one-step prediction of error variance matrix, P_kIn order to estimate the error variance matrix,_k,k-1for system noise driven arrays, Q_k-1Is a system noise matrix.

The step (4) comprises the following steps,

after the forward sequence navigation and filtering is completed, the navigation solution, dead reckoning and Kalman filtering are not stopped, but the reverse navigation solution, dead reckoning and Kalman filtering are continuously carried out, and the main processing method comprises the following steps:

1) reverse navigation

The difference between the calculation formula of navigation and forward navigation is that some state quantities need to be inverted in the calculation process, and the specific processing comprises the following steps:

a) the gravity acceleration is reversed: g is ═ g

b) Overload reversing: fb ═ fb

c) Reversing the angular velocity: wibb ═ wibb

d) The rotation angular velocity of the earth is reversed: wien ═ wien

e) The implication angular velocity is reversed: wenn ═ wenn

f) Location update reverse: speed reversal

g) Time: t is t-Tn

2) Reverse dead reckoning

The reverse dead reckoning form is the same as the forward dead reckoning form, and the difference between the forward dead reckoning form and the forward dead reckoning form is that a negative sign is taken when displacement is accumulated, namely the formula is changed into:

$\left\{\begin{array}{c}{L}_D(t-T_n)=L_D\left(t\right)+\mathrm{\Δ}{S}_{\mathrm{iN}}^n/R_M\\ {\mathrm{\λ}}_D(t-T_n)={\mathrm{\λ}}_D\left(t\right)+\left(\mathrm{\Δ}{S}_{\mathrm{iE}}^n\sec L\right)/R_N\\ h_D(t-T_n)=h_D\left(t\right)+\mathrm{\Δ}{S}_{\mathrm{iU}}^n\end{array}\right.$

3) inverse filtering

The backward filtering process is the same as the forward filtering calculation, and only all elements in the state matrix and the measurement matrix need to be inverted.

The step (5) includes that after estimation and compensation of the error term are completed by using forward and backward combined filtering, relatively accurate track information can be obtained, but since the remaining error of the odometer still has cumulation, further correction of the track is usually required through a mark point, and the specific method is as follows:

error of odometer dead reckoning is calculated at the landmark points:

$\left\{\begin{array}{c}{\mathrm{dL}}_D=L_D-L_M\\ {\mathrm{dh}}_D=h_D-h_M\\ {\mathrm{d\λ}}_D={\mathrm{\λ}}_D-{\mathrm{\λ}}_M\end{array}\right.$

wherein λ_D、L_D、h_DLongitude, latitude, and altitude for odometry dead reckoning; lambda [ alpha ]_M、L_M、h_MLongitude, latitude, and altitude at the landmark point.

Then it can be obtained

$\left[\begin{array}{c}{\mathrm{\Δ\Φ}}_u\\ \mathrm{\ΔSF}\\ {\mathrm{\Δ\Φ}}_L\end{array}\right]={\left[\begin{array}{ccc}{S}_{\mathrm{De}}& S_{\mathrm{Dn}}& \\ & S_{\mathrm{Du}}& S_{\mathrm{DL}}\\ -S_{\mathrm{Dn}}& S_{\mathrm{De}}& \end{array}\right]}^{-1}\left[\begin{array}{c}{\mathrm{dL}}_D\\ {\mathrm{dh}}_D\\ {\mathrm{d\λ}}_D\end{array}\right]$

Wherein Δ Φ_u、ΔΦ_LAnd delta SF is the residual course installation error angle, pitch installation error angle and scale coefficient error of the odometer respectively; s_Dn、S_Du、S_De、S_DLNorth direction respectively obtained for odometer dead reckoningVertical, east, and radial displacements;

after the estimated error parameter, the following judgment is made, namely when the radial error is more than 0.01m

dS_D=sqrt(dL_D*dL_D+dh_D*dh_D+dλ_D*dλ_D)>And when the distance is 0.01m, accumulating and compensating the odometer residual error obtained by calculation, and recalculating from the last mark point, wherein the compensation method of the odometer residual error comprises the following steps:

the errors are first accumulated:

$\left[\begin{array}{c}{\mathrm{d\Φ}}_u\\ \mathrm{dSF}\\ {\mathrm{d\Φ}}_L\end{array}\right]=\left[\begin{array}{c}{\mathrm{d\Φ}}_u\\ \mathrm{dSF}\\ {\mathrm{d\Φ}}_L\end{array}\right]+\left[\begin{array}{c}{\mathrm{\Δ\Φ}}_u\\ \mathrm{\ΔSF}\\ {\mathrm{\Δ\Φ}}_L\end{array}\right]$

the error is then compensated for:

$d{C}_o^n=\left[\begin{array}{ccc}1& -{\mathrm{d\Φ}}_L& {\mathrm{d\Φ}}_u\\ {\mathrm{d\Φ}}_L& 1& 0\\ -{\mathrm{d\Φ}}_u& 0& 1\end{array}\right]d{C}_o^n$

ΔS_i=(1-dSF)ΔS_i

the iterative calculation is thus performed between two marker points until the radial position error at that marker point is less than 0.01 m. And then the next marker point correction is performed.

The method has the advantages that the method recycles the whole-process test data, estimates and compensates various errors of the inertial navigation system and the odometer through forward and reverse combined navigation filtering, further corrects the track by combining the position information of the mark point, and finally obtains high-precision pipeline track data.

Drawings

FIG. 1 is a flow chart of an inertial navigation odometer combined navigation method for pipeline surveying and mapping according to the present invention.

Detailed Description

The invention is described in detail below with reference to the following figures and specific embodiments:

firstly, initial setting is carried out by utilizing position information of an initial point, then forward navigation solution, dead reckoning and Kalman filtering are carried out, closed-loop correction is carried out on three speed errors and three misalignment angles in the whole filtering process, when the data end is calculated, backward navigation solution, dead reckoning and Kalman filtering are continuously carried out, and after one-time complete forward and backward combined navigation and filtering is completed, correction is carried out on the following errors, wherein the correction comprises the following steps: and (3) performing forward navigation and filtering continuously after the gyroscope drift, the accelerometer zero offset, the odometer position error, the odometer installation error angle and the odometer scale coefficient error of the inertial navigation system, storing data such as an attitude matrix, an odometer displacement increment and a combined navigation result of the inertial navigation system, and finally further correcting the track obtained by combined navigation by combining mark point data.

Firstly establishing an error model of an inertial navigation system and a mileometer, then utilizing the whole-course data detected by a pipeline to carry out sequential forward navigation and dead reckoning, simultaneously carrying out Kalman filtering to estimate each error item, carrying out backward navigation and dead reckoning after the data is calculated to the end, continuously estimating the error by a Kalman filter, not restarting the filter until the data is positioned to the start position, then carrying out compensation and correction on each error item, and then carrying out forward navigation and filtering, at the moment, because each main error is compensated, obtaining relatively high-precision track data, and finally further correcting the obtained track by combining with mark point information.

An inertial navigation odometer integrated navigation method for pipeline surveying and mapping comprises the following steps:

1. error model

The combined navigation algorithm of the inertial navigation odometer adopts a speed + position matching Kalman filtering scheme, the scheme adopts a 19-order navigation error model, and 19 error state variables are selected as

$X=\left[\begin{array}{ccccccccccccccccccc}{\mathrm{\δV}}_n& {\mathrm{\δV}}_u& {\mathrm{\δV}}_e& {\mathrm{\Φ}}_n& {\mathrm{\Φ}}_u& {\mathrm{\Φ}}_e& {\mathrm{\δL}}_D& {\mathrm{\δh}}_D& {\mathrm{\δ\λ}}_D& {\▿}_x& {\▿}_y& {\▿}_z& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z& {\mathrm{\Θ}}_Y& {\mathrm{\Θ}}_Z& \mathrm{\ΔSF}& \mathrm{\Δt}\end{array}\right]$

The state equation is:

$\stackrel{\·}{X}=\mathrm{AX}$

wherein

$A=\left[\begin{array}{cccccc}{A}_1& A_2& 0_{3\×3}& C_b^n& 0_{3\×3}& 0_{3\×4}\\ A_3& A_4& 0_{3\×3}& 0_{3\×3}& -C_b^n& 0_{3\×4}\\ 0_{3\×3}& A_5& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& A_6\\ & & & & & \\ & & 0_{10\×19}& & & \\ & & & & & \end{array}\right]$

$A_1=\left[\begin{array}{ccc}-V_u/R_M& -V_n/R_M& -2(V_e\tan L/R_N{+\mathrm{\ω}}_{\mathrm{ie}}\sin L)\\ 2V_n/R_M& 0& 2(V_e/R_N+{\mathrm{\ω}}_{\mathrm{ie}}\cos L)\\ V_e\tan L/R_N+2{\mathrm{\ω}}_{\mathrm{ie}}\sin L& -(V_e/R_N+2{\mathrm{\ω}}_{\mathrm{ie}}\cos L)& V_n\tan L/R_N-V_u/R_N\end{array}\right]$

$A_2=\left[\begin{array}{ccc}0& -f_e& f_u\\ f_e& 0& -f_n\\ -f_u& f_n& 0\end{array}\right],$ $A_3=\left[\begin{array}{ccc}0& 0& 1/R_N\\ 0& 0& \tan L/R_N\\ -1/R_M& 0& 0\end{array}\right]$

$A_4=\left[\begin{array}{ccc}0& -V_n/R_M& -{\mathrm{\ω}}_{\mathrm{ie}}\sin L-V_e\tan L/R_N\\ V_n/R_M& 0& {\mathrm{\ω}}_{\mathrm{ie}}\cos L+V_e/R_N\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L+V_e\tan L/R_N& -{\mathrm{\ω}}_{\mathrm{ie}}\cos L/-V_E/R_N& 0\end{array}\right]$

$A_5=\left[\begin{array}{ccc}0& -V_{\mathrm{De}}& V_{\mathrm{Du}}\\ V_{\mathrm{De}}& 0& -V_{\mathrm{Dn}}\\ -V_{\mathrm{Du}}& V_{\mathrm{Dn}}& 0\end{array}\right],$ $A_6=\left[\begin{array}{cccc}-C_b^n\left(\mathrm{1,3}\right)V_D& C_b^n\left(\mathrm{1,2}\right)V_D& C_b^n\left(\mathrm{1,1}\right)V_D& 0\\ -C_b^n\left(\mathrm{2,3}\right)V_D& C_b^n\left(\mathrm{2,2}\right)V_D& C_b^n\left(\mathrm{2,1}\right)V_D& 0\\ -C_b^n\left(\mathrm{3,3}\right)V_D& C_b^n\left(\mathrm{3,2}\right)V_D& C_b^n\left(\mathrm{3,1}\right)V_D& 0\end{array}\right]$

Wherein V_n、V_u、V_eThe inertial navigation north, vertical and east speed errors are obtained; phi_n、Φ_u、Φ_eThe inertial navigation north, vertical and east misalignment angles are adopted;inertial navigation X, Y, Z axis accelerometer zero offset;_x、_y、_z: inertial navigation X, Y, Z axis gyro drift; l is_D、h_D、λ_DLatitude, altitude, longitude errors for odometer dead reckoning; theta_Y、Θ_ZThe mounting error angle between the inertial navigation and the odometer along the inertial navigation Y and Z axes; Δ SF: odometer scale coefficient error; Δ t: is a time delay; f. of_n、f_u、f_eThe specific force in the north heaven-east direction measured by the inertial navigation system; v_n、V_u、V_eRespectively the north, vertical and east speeds of the inertial navigation system; omega_ieIs the earth rotation angular rate; l represents the latitude of the position where the inertial navigation system is located; r_N、R_MRespectively the curvature radius of the Mao-unitary ring, the curvature radius in the meridian plane, V_DIs the forward speed of the odometer; v_Dn、V_Du、V_DeRespectively representing the north, vertical and east speeds of the odometer dead reckoning;representation matrixThe elements corresponding to row 1 and column 1 have the same meaning as the remaining same form variables.

2) Equation of observation

The observation equation of the filter is divided into two parts, and when the speed is observed, the observation equation is as follows:

$\left[\begin{array}{c}{\mathrm{\δV}}_n\\ {\mathrm{\δV}}_u\\ {\mathrm{\δV}}_e\\ {\mathrm{\δL}}_D\\ {\mathrm{\δh}}_D\\ {\mathrm{\δ\λ}}_D\end{array}\right]=\left[\begin{array}{ccccccccccc}1& 0& 0& 0& V_{\mathrm{De}}& -V_{\mathrm{Du}}& & C_b^n\left(\mathrm{1,3}\right)V_D& -C_b^n(1,2)V_D& -C_b^n\left(\mathrm{1,1}\right)V_D& {\stackrel{\·}{V}}_n\\ 0& 1& 0& -V_{\mathrm{De}}& 0& V_{\mathrm{Dn}}& 0_{3\×9}& C_b^n\left(\mathrm{2,3}\right)V_D& -C_b^n\left(\mathrm{2,2}\right)V_D& -C_b^n(2,1)V_D& {\stackrel{\·}{V}}_u\\ 0& 0& 1& V_{\mathrm{Du}}& -V_{\mathrm{Dn}}& 0& & C_b^n\left(\mathrm{3,3}\right)V_D& -C_b^n\left(\mathrm{2,2}\right)V_D& -C_b^n\left(\mathrm{3,1}\right)V_D& {\stackrel{\·}{V}}_e\\ & & & & & & & & & & \\ & & & & & & 0_{3\×19}& & & & \\ & & & & & & & & & & \end{array}\right]X---\left(1\right)$ when reference point information exists, the observation equation is as follows:

$\left[\begin{array}{c}{\mathrm{\δV}}_n\\ {\mathrm{\δV}}_u\\ {\mathrm{\δV}}_e\\ {\mathrm{\δL}}_D\\ {\mathrm{\δh}}_D\\ {\mathrm{\δ\λ}}_D\end{array}\right]=\begin{array}{c}\left[\begin{array}{ccccc}& & & & \\ & & 0_{3\×19}& & \\ & & & & \\ & 1& 0& 0& \\ 0_{3\×6}& 0& 1& 0& 0_{3\×10}\\ & 0& 0& 1& \end{array}\right]X---\left(2\right)\end{array}$

respectively representing the acceleration values of the inertial navigation system geographic system used for updating the speed during navigation resolving; the remaining variables are defined in the above section.

3) Equation discretization

The state transition matrix discretization formula is as follows

${\mathrm{\φ}}_{k+1,k}=I+T_n{A}_{T_n}+T_n{A}_{2T_n}+......+T_n{A}_{T_e}=I+\underset{t=T_n}{\overset{T_e}{\mathrm{\Σ}}}{T}_n{A}_t---\left(3\right)$

Wherein, T_nFor navigation period, T_n=0.005s，T_eFor the filter period, T_e=1s。A_tFor the state transition matrix at time t, phi_k+1,kFor the transition matrix after discretization, t =0 at the beginning of each filtering cycle.

The discretization formula of the noise array is as follows:

Q_k=QT_e

q is a noise matrix.

The observed quantity solving formula is as follows:

$\left\{\begin{array}{c}{\mathrm{\δV}}_n=V_n-V_{\mathrm{Dn}}\\ {\mathrm{\δV}}_u=V_u-V_{\mathrm{Dn}}\\ {\mathrm{\δV}}_e=V_e-V_{\mathrm{De}}\\ {\mathrm{\δL}}_D=L_D-L_M\\ {\mathrm{\δh}}_D=h_D-h_M\\ {\mathrm{\δ\λ}}_D={\mathrm{\λ}}_D-{\mathrm{\λ}}_M\end{array}\right.---\left(4\right)$

wherein λ is_D、L_D、h_DLongitude, latitude, and altitude for odometry dead reckoning; lambda [ alpha ]_M、L_M、h_MLongitude, latitude, and altitude at the landmark point. The odometer speed is given by:

$\left[\begin{array}{c}{V}_{\mathrm{Dn}}\\ V_{\mathrm{Du}}\\ V_{\mathrm{De}}\end{array}\right]=C_b^n\left[\begin{array}{c}\mathrm{\ΔS}/\mathrm{dT}\\ 0\\ 0\end{array}\right]---\left(5\right)$

in the formula, Δ S is a displacement increment within 0.1S, and dT is 0.1S.

2. Dead reckoning

And carrying out dead reckoning while carrying out inertial navigation. The calculation period is the same as the navigation resolving period, Tn =5 (ms); the dead reckoning position information is initialized and calculated with navigation, namely initial longitude, latitude and height are obtained by utilizing external binding values.

The displacement of the output carrier of the mileage meter in the ith sampling period is designed to be delta S_iThen, the expression in the inertial navigation carrier coordinate system b is:

$\mathrm{\Δ}{S}_i^b={\left[\begin{array}{ccc}\mathrm{\Δ}{S}_i& 0& 0\end{array}\right]}^T---\left(16\right)$

converting the attitude matrix of the inertial navigation system into a geographic coordinate system to obtain:

$\mathrm{\Δ}{S}_i^n=C_b^n\mathrm{\Δ}{S}_i^b---\left(7\right)$

to eliminate the odometry noise, the speed is smoothed in a 0.1s cycle.

The dead reckoning formula is as follows:

$\left\{\begin{array}{c}{L}_D\left(t\right)=L_D(t-T_n)+\mathrm{\Δ}{S}_{\mathrm{iN}}^n/R_M\\ {\mathrm{\λ}}_D\left(t\right)={\mathrm{\λ}}_D(t-T_n)+\left(\mathrm{\Δ}{S}_{\mathrm{iE}}^n\sec L\right)/R_N\\ h_D\left(t\right)=h_D(t-T_n)+\mathrm{\Δ}{S}_{\mathrm{iU}}^n\end{array}\right.---\left(9\right)$

respectively representing the displacement increment of the odometer geographic system in the navigation resolving period.

Kalman filtering model

The Kalman filter equation takes the form of the document Kalman filter and integrated navigation principles (first edition, edited by qinyuan et al), and has the following concrete formula:

state one-step prediction

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}---\left(9\right)$

State estimation

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k[Z_k-H_k{\hat{X}}_{k,k-1}]---\left(10\right)$

Filter gain matrix

$K_k=P_{k,k-1}{H}_k^T{[H_k{P}_{k,k-1}{H}_k^T+R_k]}^{-1}---\left(11\right)$

One-step prediction error variance matrix

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k,k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k,k-1}^T---\left(12\right)$

Estimation error variance matrix

P_k=[I-K_kH_k]P_k,k-1(13)

Wherein,in order to predict the value of the one-step state,estimate the matrix for the state, phi_k,k-1For a state one-step transition matrix, H_kFor measuring the matrix, Z_kMeasurement of quantitative value, K_kFor filtering the gain matrix, R_kFor observing noise arrays, P_k,k-1For one-step prediction of error variance matrix, P_kIn order to estimate the error variance matrix,_k,k-1for system noise driven arrays, Q_k-1Is a system noise matrix.

4. Reverse processing

After the forward sequence navigation and filtering is completed, the navigation solution, dead reckoning and Kalman filtering are not stopped, but the reverse navigation solution, dead reckoning and Kalman filtering are continuously carried out, and the main processing method comprises the following steps:

1) reverse navigation

The difference between the calculation formula of navigation and forward navigation is that some state quantities need to be inverted in the calculation process, and the specific processing comprises the following steps:

h) the gravity acceleration is reversed: g is ═ g

i) Overload reversing: fb ═ fb

j) Reversing the angular velocity: wibb ═ wibb

k) The rotation angular velocity of the earth is reversed: wien ═ wien

l) involved angular velocity reversal: wenn ═ wenn

m) location update reverse: speed reversal

n) time: t is t-Tn

2) Reverse dead reckoning

The reverse dead reckoning form is the same as the forward dead reckoning form, and the difference between the forward dead reckoning form and the forward dead reckoning form is that a negative sign is taken when displacement is accumulated, namely the formula is changed into:

$\left\{\begin{array}{c}{L}_D(t-T_n)=L_D\left(t\right)+\mathrm{\Δ}{S}_{\mathrm{iN}}^n/R_M\\ {\mathrm{\λ}}_D(t-T_n)={\mathrm{\λ}}_D\left(t\right)+\left(\mathrm{\Δ}{S}_{\mathrm{iE}}^n\sec L\right)/R_N\\ h_D(t-T_n)=h_D\left(t\right)+\mathrm{\Δ}{S}_{\mathrm{iU}}^n\end{array}\right.---\left(14\right)$

3) inverse filtering

The backward filtering process is the same as the forward filtering calculation, and only all elements in the state matrix and the measurement matrix need to be inverted.

5. Landmark correction

After estimation and compensation of the error term are completed by forward and backward combined filtering, relatively accurate track information can be obtained, but since the remaining error of the odometer still has cumulation, further correction of the track is usually required through a mark point, and a specific method is as follows.

Error of odometer dead reckoning is calculated at the landmark points:

$\left\{\begin{array}{c}{\mathrm{dL}}_D=L_D-L_M\\ {\mathrm{dh}}_D=h_D-h_M\\ {\mathrm{d\λ}}_D={\mathrm{\λ}}_D-{\mathrm{\λ}}_M\end{array}\right.---\left(15\right)$

wherein λ_D、L_D、h_DLongitude, latitude, and altitude for odometry dead reckoning; lambda [ alpha ]_M、L_M、h_MLongitude, latitude, and altitude at the landmark point.

Then it can be obtained

$\left[\begin{array}{c}{\mathrm{\Δ\Φ}}_u\\ \mathrm{\ΔSF}\\ {\mathrm{\Δ\Φ}}_L\end{array}\right]={\left[\begin{array}{ccc}{S}_{\mathrm{De}}& S_{\mathrm{Dn}}& \\ & S_{\mathrm{Du}}& S_{\mathrm{DL}}\\ -S_{\mathrm{Dn}}& S_{\mathrm{De}}& \end{array}\right]}^{-1}\left[\begin{array}{c}{\mathrm{dL}}_D\\ {\mathrm{dh}}_D\\ {\mathrm{d\λ}}_D\end{array}\right]---\left(16\right)$

Wherein Δ Φ_u、ΔΦ_LAnd delta SF is the residual course installation error angle, pitch installation error angle and scale coefficient error of the odometer respectively; s_Dn、S_Du、S_De、S_DLRespectively obtaining north, vertical, east and radial displacements by odometer dead reckoning;

after the estimated error parameter, the following judgment is made, namely when the radial error is more than 0.01m

dS_D=sqrt(dL_D*dL_D+dh_D*dh_D+dλ_D*dλ_D)>0.01m

And accumulating and compensating the odometer residual error obtained by calculation, and recalculating from the last mark point, wherein the compensation method of the odometer residual error comprises the following steps:

the errors are first accumulated:

$\left[\begin{array}{c}{\mathrm{d\Φ}}_u\\ \mathrm{dSF}\\ {\mathrm{d\Φ}}_L\end{array}\right]=\left[\begin{array}{c}{\mathrm{d\Φ}}_u\\ \mathrm{dSF}\\ {\mathrm{d\Φ}}_L\end{array}\right]+\left[\begin{array}{c}{\mathrm{\Δ\Φ}}_u\\ \mathrm{\ΔSF}\\ {\mathrm{\Δ\Φ}}_L\end{array}\right]---\left(17\right)$

the error is then compensated for:

$d{C}_o^n=\left[\begin{array}{ccc}1& -{\mathrm{d\Φ}}_L& {\mathrm{d\Φ}}_u\\ {\mathrm{d\Φ}}_L& 1& 0\\ -{\mathrm{d\Φ}}_u& 0& 1\end{array}\right]d{C}_o^n$

ΔS_i=(1-dSF)ΔS_i(18)

the iterative calculation is thus performed between two marker points until the radial position error at that marker point is less than 0.01 m. And then the next marker point correction is performed.

Claims (1)

1. An inertial navigation odometer combined navigation method for pipeline surveying and mapping is characterized in that: which comprises the following steps of,

(1) a calculation model is established, and the calculation model is established,

(2) the calculation of the position of the ship is carried out,

(3) a Kalman filter model is used to model the Kalman filter,

(4) the reverse treatment is carried out on the mixture,

(5) correcting the mark points;

the step (1) comprises the following steps,

1) error model

The combined navigation algorithm of the inertial navigation odometer adopts a velocity and position matching Kalman filtering method, the scheme adopts a 19-order navigation error model, and 19 error state variables are selected as

$X=\left[\begin{array}{ccccccccccccccccccc}{\mathrm{\δV}}_n& {\mathrm{\δV}}_u& {\mathrm{\δV}}_e& {\mathrm{\Φ}}_n& {\mathrm{\Φ}}_u& {\mathrm{\Φ}}_e& {\mathrm{\δL}}_D& {\mathrm{\δh}}_D& {\mathrm{\δ\λ}}_D& {\▿}_x& {\▿}_y& {\▿}_z& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z& {\mathrm{\Θ}}_Y& {\mathrm{\Θ}}_Z& \mathrm{\Δ}SF& \mathrm{\Δ}t\end{array}\right]$

The state equation is:

$\stackrel{\·}{X}=AX$

wherein

$A=\left[\begin{array}{cccccc}{A}_1& A_2& 0_{3\×3}& C_b^n& 0_{3\×3}& 0_{3\×4}\\ A_3& A_4& 0_{3\×3}& 0_{3\×3}& -C_b^n& 0_{3\×4}\\ 0_{3\×3}& A_5& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& A_6\\ & & & & & \\ & & 0_{10\×19}& & & \\ & & & & & \end{array}\right]$

$A_1=\left[\begin{array}{ccc}-V_u/R_M& -V_u/R_M& -2(V_e\tan L/R_N+{\mathrm{\ω}}_{ie}\sin L)\\ 2V_n/R_M& 0& 2(V_e/R_N+{\mathrm{\ω}}_{ie}\cos L)\\ V_e\tan L/R_N+2{\mathrm{\ω}}_{ie}\sin L& -(V_e/R_N+2{\mathrm{\ω}}_{ie}\cos L)& V_n\tan L/R_N-V_u/R_N\end{array}\right]$

$A_2=\left[\begin{array}{ccc}0& -f_e& f_u\\ f_e& 0& -f_n\\ -f_u& f_n& 0\end{array}\right],A_3=\left[\begin{array}{ccc}0& 0& 1/R_N\\ 0& 0& \tan L/R_N\\ -1/R_M& 0& 0\end{array}\right]$

$A_4=\left[\begin{array}{ccc}0& -V_n/R_M& -{\mathrm{\ω}}_{ie}\sin L-V_e\tan L/R_N\\ V_n/R_M& 0& {\mathrm{\ω}}_{ie}\cos L+V_e/R_N\\ {\mathrm{\ω}}_{ie}\sin L+V_e\tan L/R_N& -{\mathrm{\ω}}_{ie}\cos L-V_E/R_N& 0\end{array}\right]$

$A_5=\left[\begin{array}{ccc}0& -V_{De}& V_{Du}\\ V_{De}& 0& -V_{Dn}\\ -V_{Du}& V_{Dn}& 0\end{array}\right],A_6=\left[\begin{array}{cccc}-C_b^n(1,3)V_D& C_b^n(1,2)V_D& C_b^n(1,1)V_D& 0\\ -C_b^n(2,3)V_D& C_b^n(2,2)V_D& C_b^n(2,1)V_D& 0\\ -C_b^n(3,3)V_D& C_b^n(3,2)V_D& C_b^n(3,1)V_D& 0\end{array}\right]$

Wherein, V_n、V_u、V_eThe inertial navigation north, vertical and east speed errors are obtained; phi_n、Φ_u、Φ_eThe inertial navigation north, vertical and east misalignment angles are adopted;inertial navigation X, Y, Z axis accelerometer zero offset;_x、_y、_z: inertial navigation X, Y, Z axis gyro drift; l is_D、h_D、λ_DLatitude, altitude, longitude errors for odometer dead reckoning; theta_Y、Θ_ZThe mounting error angle between the inertial navigation and the odometer along the inertial navigation Y and Z axes; Δ SF: odometer scale coefficient error; Δ t: is a time delay; f. of_n、f_u、f_eThe specific force in the north heaven-east direction measured by the inertial navigation system; v_n、V_u、V_eRespectively the north, vertical and east speeds of the inertial navigation system; omega_ieIs the earth rotation angular rate; l represents the latitude of the position where the inertial navigation system is located; r_N、R_MRespectively the curvature radius of the Mao-unitary ring, the curvature radius in the meridian plane, V_DIs the forward speed of the odometer; v_Dn、V_Du、V_DeRespectively representing the north, vertical and east speeds of the odometer dead reckoning;representation matrixThe elements corresponding to the 1 st row and the 1 st column of the 1 st row have the same meanings as the rest of the same-form variables;

2) equation of observation

The observation equation of the filter is divided into two parts, and when the speed is observed, the observation equation is as follows:

$\left[\begin{array}{c}\mathrm{\δ}{V}_n\\ {\mathrm{\δV}}_u\\ {\mathrm{\δV}}_e\\ {\mathrm{\δL}}_D\\ {\mathrm{\δh}}_D\\ {\mathrm{\δ\λ}}_D\end{array}\right]=\left[\begin{array}{ccccccccccc}1& 0& 0& 0& V_{De}& -V_{Du}& & C_b^n(1,3)V_D& -C_b^n(1,2)V_D& -C_b^n(1,1)V_D& {\stackrel{\·}{V}}_n\\ 0& 1& 0& -V_{De}& 0& V_{Dn}& 0_{3\×9}& C_b^n(2,3)V_D& -C_b^n(2,2)V_D& -C_b^n(2,1)V_D& {\stackrel{\·}{V}}_u\\ 0& 0& 1& V_{Du}& -V_{Dn}& 0& & C_b^n(3,3)V_D& -C_b^n(2,2)V_D& -C_b^n(3,1)V_D& {\stackrel{\·}{V}}_e\\ & & & & & & & & & & \\ & & & & & & 0_{3\×19}& & & & \\ & & & & & & & & & & \end{array}\right]X$

when reference point information exists, the observation equation is as follows:

$\left[\begin{array}{c}\mathrm{\δ}{V}_n\\ {\mathrm{\δV}}_u\\ {\mathrm{\δV}}_e\\ {\mathrm{\δL}}_D\\ {\mathrm{\δh}}_D\\ {\mathrm{\δ\λ}}_D\end{array}\right]=\left[\begin{array}{ccccc}& & & & \\ & & 0_{3\×19}& & \\ & & & & \\ & 1& 0& 0& \\ 0_{3\×6}& 0& 1& 0& 0_{3\×10}\\ & 0& 0& 1& \end{array}\right]X$

respectively representing the acceleration values of the inertial navigation system geographic system used for updating the speed during navigation resolving; the other variables are defined in the same section as above;

3) equation discretization

The state transition matrix discretization formula is as follows

${\mathrm{\φ}}_{k+1,k}=I+T_n{A}_{T_n}+T_n{A}_{2T_n}+......+T_n{A}_{T_e}=I+\underset{t=T_n}{\overset{T_e}{\Σ}}{T}_n{A}_t$

Wherein, T_nFor navigation period, T_n＝0.005s，T_eFor the filter period, T_e＝1s，A_tFor the state transition matrix at time t, phi_k+1,kFor the transition matrix after discretization, t is 0 at the beginning of each filtering period;

the discretization formula of the noise array is as follows:

Q_k＝QT_e

q is a noise matrix, and Q is a noise matrix,

the observed quantity solving formula is as follows:

$\left\{\begin{array}{c}\mathrm{\δ}{V}_n=V_n-V_{Dn}\\ {\mathrm{\δV}}_u=V_u-V_{Du}\\ {\mathrm{\δV}}_e=V_e-V_{De}\\ {\mathrm{\δL}}_D=L_D-L_M\\ {\mathrm{\δh}}_D=h_D-h_M\\ {\mathrm{\δ\λ}}_D={\mathrm{\λ}}_D-{\mathrm{\λ}}_M\end{array}\right.$

wherein λ is_D、L_D、h_DLongitude, latitude, and altitude for odometry dead reckoning; lambda [ alpha ]_M、L_M、h_MThe velocity of the odometer, for longitude, latitude, altitude at the landmark point, is given by:

$\left[\begin{array}{c}{V}_{Dn}\\ V_{Du}\\ V_{De}\end{array}\right]=C_b^n\left[\begin{array}{c}\mathrm{\Δ}S/dT\\ 0\\ 0\end{array}\right]$

wherein Δ S is a displacement increment within 0.1S, and dT is 0.1S;

the step (2) comprises the following steps,

carrying out dead reckoning while carrying out inertial navigation, wherein the calculation period is the same as the navigation calculation period, and Tn is 5 (ms); the dead reckoning position information is initialized and calculated with navigation, namely the initial longitude, latitude and height are obtained by utilizing the external binding value,

the displacement of the output carrier of the mileage meter in the ith sampling period is designed to be delta S_iThen, the expression in the inertial navigation carrier coordinate system b is:

${\mathrm{\ΔS}}_i^b={\left[\begin{array}{ccc}{\mathrm{\ΔS}}_i& 0& 0\end{array}\right]}^T$

converting the attitude matrix of the inertial navigation system into a geographic coordinate system to obtain:

${\mathrm{\ΔS}}_i^n=C_b^n{\mathrm{\ΔS}}_i^b$

in order to eliminate the mileage metering noise, the speed is smoothed in 0.1s period,

the dead reckoning formula is as follows:

$\left\{\begin{array}{c}{L}_D\left(t\right)=L_D(t-T_n)+\mathrm{\Δ}{S}_{iN}^n/R_M\\ {\mathrm{\λ}}_D\left(t\right)={\mathrm{\λ}}_D(t-T_n)+\left({\mathrm{\ΔS}}_{iE}^n\sec L\right)/R_N\\ h_D\left(t\right)=h_D(t-T_n)+{\mathrm{\ΔS}}_{iU}^n\end{array}\right.$

respectively representing the displacement increment of the odometer geographic system in the navigation resolving period;

the step (3) comprises the following formula of Kalman filtering equation:

state one-step prediction

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}$

State estimation

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k\[Z_k-H_k{\hat{X}}_{k,k-1}\]$

Filter gain matrix

$K_k=P_{k,k-1}{H}_k^T{\[H_k{P}_{k,k-1}{H}_k^T+R_k\]}^{-1}$

One-step prediction error variance matrix

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k,k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k,k-1}^T$

Estimation error variance matrix

P_k＝[I-K_kH_k]P_k,k-1

Wherein,in order to predict the value of the one-step state,estimate the matrix for the state, phi_k,k-1For a state one-step transition matrix, H_kFor measuring the matrix, Z_kMeasurement of quantitative value, K_kFor filtering the gain matrix, R_kFor observing noise arrays, P_k,k-1For one-step prediction of error variance matrix, P_kIn order to estimate the error variance matrix,_k,k-1for system noise driven arrays, Q_k-1A system noise array;

the step (4) comprises the following steps,

after the forward sequence navigation and filtering is completed, the navigation solution, dead reckoning and Kalman filtering are not stopped, but the reverse navigation solution, dead reckoning and Kalman filtering are continuously carried out, and the main processing method comprises the following steps:

1) reverse navigation

The difference between the calculation formula of navigation and forward navigation is that some state quantities need to be inverted in the calculation process, and the specific processing comprises the following steps:

a) the gravity acceleration is reversed: g is ═ g

b) Overload reversing: fb ═ fb

c) Reversing the angular velocity: wibb ═ wibb

d) The rotation angular velocity of the earth is reversed: wien ═ wien

e) The implication angular velocity is reversed: wenn ═ wenn

f) Location update reverse: speed reversal

g) Time: t is t-Tn

2) Reverse dead reckoning

The reverse dead reckoning form is the same as the forward dead reckoning form, and the difference between the forward dead reckoning form and the forward dead reckoning form is that a negative sign is taken when displacement is accumulated, namely the formula is changed into:

$\left\{\begin{array}{c}{L}_D(t-T_n)=L_D\left(t\right)-{\mathrm{\ΔS}}_{iN}^n/R_M\\ {\mathrm{\λ}}_D(t-T_n)={\mathrm{\λ}}_D\left(t\right)-\left({\mathrm{\ΔS}}_{iE}^n\sec L\right)/R_N\\ h_D(t-T_n)=h_D\left(t\right)-{\mathrm{\ΔS}}_{iU}^n\end{array}\right.$

3) inverse filtering

The backward filtering process is the same as the forward filtering calculation, and only all elements in the state matrix and the measurement matrix need to be inverted;

the step (5) includes that after estimation and compensation of the error term are completed by using forward and backward combined filtering, relatively accurate track information can be obtained, but since the remaining error of the odometer still has cumulation, further correction of the track is usually required through a mark point, and the specific method is as follows:

error of odometer dead reckoning is calculated at the landmark points:

$\left\{\begin{array}{c}d{L}_D=L_D-L_M\\ {\mathrm{dh}}_D=h_D-h_M\\ {\mathrm{d\λ}}_D={\mathrm{\λ}}_D-{\mathrm{\λ}}_M\end{array}\right.$

wherein λ_D、L_D、h_DLongitude, latitude, and altitude for odometry dead reckoning; lambda [ alpha ]_M、L_M、h_MThe longitude, latitude and altitude at the mark point,

then it can be obtained

$\left[\begin{array}{c}\mathrm{\Δ}{\mathrm{\Φ}}_u\\ \mathrm{\Δ}SF\\ \mathrm{\Δ}{\mathrm{\Φ}}_L\end{array}\right]={\left[\begin{array}{ccc}{S}_{De}& S_{Dn}& \\ & S_{Du}& S_{DL}\\ -S_{Dn}& S_{De}& \end{array}\right]}^{-1}\left[\begin{array}{c}d{L}_D\\ {\mathrm{dh}}_D\\ {\mathrm{d\λ}}_D\end{array}\right]$

Wherein Δ Φ_u、ΔΦ_LAnd delta SF is the residual course installation error angle, pitch installation error angle and scale coefficient error of the odometer respectively; s_Dn、S_Du、S_De、S_DLRespectively obtaining north, vertical, east and radial displacements by odometer dead reckoning;

after the estimated error parameter, the following judgment is made, namely when the radial error is more than 0.01m

dS_D＝sqrt(dL_D*dL_D+dh_D*dh_D+dλ_D*dλ_D)>And when the distance is 0.01m, accumulating and compensating the odometer residual error obtained by calculation, and recalculating from the last mark point, wherein the compensation method of the odometer residual error comprises the following steps:

the errors are first accumulated:

$\left[\begin{array}{c}d{\mathrm{\Φ}}_u\\ dSF\\ d{\mathrm{\Φ}}_L\end{array}\right]=\left[\begin{array}{c}d{\mathrm{\Φ}}_u\\ dSF\\ d{\mathrm{\Φ}}_L\end{array}\right]+\left[\begin{array}{c}\mathrm{\Δ}{\mathrm{\Φ}}_u\\ \mathrm{\Δ}SF\\ \mathrm{\Δ}{\mathrm{\Φ}}_L\end{array}\right]$

the error is then compensated for:

${\mathrm{dC}}_o^n=\left[\begin{array}{ccc}1& -{\mathrm{d\Φ}}_L& {\mathrm{d\Φ}}_u\\ {\mathrm{d\Φ}}_L& 1& 0\\ -{\mathrm{d\Φ}}_u& 0& 1\end{array}\right]{\mathrm{dC}}_o^n$

ΔS_i＝(1-dSF)ΔS_i

iterative calculation is carried out between two mark points until the radial position error of the mark point is less than 0.01m, and then the next mark point correction is carried out.