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

A kind of pipeline mapping inertial navigation odometer Combinated navigation method

Technical field

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 can apply at oil and gas pipeline survey field, can effectively utilize detection data to inertial navigation system and the error of odometer Accurately estimate and compensate, substantially increasing the certainty of measurement of pipeline track.

Background technology

In order to ensure oil and gas pipeline safety, need periodically oil and gas pipes to be detected, to obtain pipeline track Etc. data.Typically it is laid on below earth's surface due to oil and gas pipes, is difficult to by effective high-precision positioner its concrete position Put and pipeline track measures.And inertial navigation system is the measurement apparatus of a kind of relative efficiency, there is full independence, not by outward The features such as boundary's conditionality, but inertial navigation system positioning precision dissipates in time, is combined hence with odometer auxiliary inertial navigation Navigation is a kind of method of effective raising integrated navigation precision, and pipe detection is typically after obtaining omnidistance data to carry out Process afterwards, the most how to utilize omnidistance measurement data to obtain in the primary study that high-precision measurement track is pipe detection Hold.

" integrated navigation technology application in oil and gas pipes mapping system " (Yue Bujiang etc., China's inertial technology journal, the Volume 16 the 6th phase, in December, 2008) propose a kind of integrated navigation technology in oil and gas pipes is surveyed and drawn, apply forward filtering Completing estimation error, and utilize smoothing technique to be modified the error of initial time, the method is equivalent to make use of the most complete Number of passes evidence completes the estimation error in each moment, the method increases the filtering accuracy of unidirectional filtering start time.In order to more Make full use of whole process test data, the present invention propose a kind of utilize forward and reverse combine navigation and filtering method complete inertial navigation system System and the estimation of each margin of error of odometer and the method for compensation, the method is equivalent to make use of twice omnidistance data every to complete The estimation of error, has higher error estimation accuracy relative to the method in document, and finally the position in conjunction with index point is believed Gained track is modified, to improve trace information further by breath.

Summary of the invention

It is an object of the invention to provide a kind of side that inertial navigation system and odometer error effectively can be estimated and compensated Method, the omnidistance detection data of pipeline can be carried out navigating and filtering forward and reverse associating, complete the estimation to each error term by the method And compensation, to obtain high-precision pipeline trace information.

The present invention is achieved in that a kind of pipeline mapping inertial navigation odometer Combinated navigation method, and it includes walking as follows Suddenly,

(1) computation model is set up,

(2) boat position calculates,

(3) Kalman filter model,

(4) reverse process,

(5) index point correction.

Described step (1) includes,

1) error model

Inertial navigation odometer Integrated Navigation Algorithm uses " speed+position " coupling Kalman filter method, and the program uses 19 Rank navigation error model, choosing 19 error state variable is

$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]$

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_eFor inertial navigation north orientation, vertical, east orientation velocity error；Φ_n、Φ_u、Φ_eFor inertial navigation north orientation, hang down To, east orientation misalignment；Inertial navigation X, Y, Z axis accelerometer bias；ε_x、ε_y、ε_z: inertial navigation X, Y, Z axis gyro floats Move；δL_D、δh_D、δλ_DFor odometer dead reckoning latitude, highly, longitude error；Θ_Y、Θ_ZUsed for edge between inertial navigation and odometer Lead the fix error angle of Y and Z axis；Δ SF: odometer scale coefficient error；Δ t: for time delay；f_n、f_u、f_eFor inertial navigation system The northern sky east tripartite measured specific force upwards；V_n、V_u、V_eIt is respectively inertial navigation system north orientation, vertical and east orientation speed；ω_ieFor the earth Angle of rotation speed；L represents the latitude of inertial navigation system present position；R_N、R_MIt is respectively radius of curvature in prime vertical, meridian plane incurvature Radius, V_DForward speed for odometer；V_Dn、V_Du、V_DeRepresent the north orientation of odometer dead reckoning respectively, vertical, east orientation is fast Degree；Representing matrixThe element that 1st row the 1st row are corresponding, the implication of remaining same form variable is identical with this；

2) observational equation

Wave filter observational equation is divided into two parts, and when there being speed to observe, observational 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 there being reference point information, observational 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}$

The accekeration of the inertial navigation system Department of Geography updated for speed when representing navigation calculation respectively； Remaining each variable-definition ibid saves；

3) equation discretization

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 cycle, T_n=0.005s, T_eFor filtering cycle, T_e=1s, A_tFor the state-transition matrix of t, φ_k+1,kFor the transfer matrix after discretization, t=0 when each filtering cycle starts；

Noise battle array discretization formula is:

Q_k=QT_e

Q is noise battle array,

Observed quantity solution 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, λ_D、L_D、h_DFor the longitude of odometer dead reckoning, latitude, highly；λ_M、L_M、h_MFor the longitude at index point, Latitude, highly, the speed of odometer is obtained by following formula:

$\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 formula, Δ S is the displacement increment in 0.1s, dT=0.1s.

Described step (2) includes,

Carrying out dead reckoning while carrying out inertial navigation, the cycle that calculates is with navigation calculation cycle, Tn=5 (ms)；Boat position pushes away Calculate positional information and initialize same navigation calculation, i.e. utilize extraneous bookbinding value to obtain initial longitude, latitude and height,

If the displacement that odometer exports carrier in the i-th sampling period is Δ S_i, then it is in inertial navigation carrier coordinate system b system It is expressed as:

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

The attitude matrix utilizing inertial navigation system converts it under geographic coordinate system:

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

For eliminating odometer quantizing noise, for the cycle, speed is made smoothing processing with 0.1s.

Dead reckoning formula is:

$\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.$

The displacement increment of odometer Department of Geography in the expression navigation calculation cycle respectively.

Described step (3) includes, the formula of Kalman filter equation is as follows:

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}]$

Filtering 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 covariance 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 battle array

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

Wherein,It is a step status predication value,For state estimation matrix, Φ_k,k-1For state Matrix of shifting of a step, H_kFor measurement matrix, Z_kFor measurement, K_kFor filtering gain matrix, R_kFor observation noise battle array, P_k,k-1For one-step prediction error variance Battle array, P_kFor estimation error variance battle array, Γ_k,k-1Battle array, Q is driven for system noise_k-1For system noise acoustic matrix.

Described step (4) includes,

After the navigation completing forward sequence and filtering, navigation calculation, dead reckoning and Kalman filter do not stop, But proceed reverse navigation calculation, dead reckoning and Kalman filter, main methods includes:

1) reverse navigation

The computing formula of navigation, with positive navigation, is a difference in that during calculating and needs to take some quantity of states Instead, concrete process includes:

A) acceleration of gravity is reverse: g=-g

B) overload is reversely: fb=-fb

C) angular speed is reverse: wibb=-wibb

D) rotational-angular velocity of the earth is reverse: wien=-wien

E) angular speed is involved reverse: wenn=-wenn

F) location updating is reverse: speed negates

G) time: t=t-Tn

2) reversely dead reckoning

Reversely the form of dead reckoning is with the dead reckoning of forward, and the difference with forward dead reckoning is when displacement adds up Take negative sign, i.e. formula becomes:

$\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

Inverse filtering flow process calculates with forward filtering, only need to be negated i.e. by all elements in state matrix and measurement matrix Can.

Described step (5) includes, after utilizing forward and reverse Federated filter to complete the estimation to error term and compensate, i.e. Available relatively accurate trace information, but owing to the remainder error of odometer still has additive, therefore typically require Further being revised track by index point, concrete method is as follows:

The error of calculating odometer dead reckoning at index point:

$\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_DFor the longitude of odometer dead reckoning, latitude, highly；λ_M、L_M、h_MFor the longitude at index point, Latitude, highly.

Then can obtain

$\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、ΔΦ_L, Δ SF be respectively odometer remaining course fix error angle, pitching fix error angle with And scale coefficient error；S_Dn、S_Du、S_De、S_DLIt is respectively north orientation, vertical, east orientation and radial direction position that odometer dead reckoning obtains Move；

After the error parameter estimated, make the following judgment, when radial error is more than 0.01m, i.e.

dS_D=sqrt(dL_D*dL_D+dh_D*dh_D+dλ_D*dλ_D) > 0.01m time, to calculate obtain odometer remainder error enter Row is cumulative and compensates, and re-starts calculating at a upper index point, and the compensation method of odometer remainder error is as follows:

First error is added up:

$\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]$

Then error is compensated:

$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

So it is iterated calculating between two index points, until the radial position error at this index point is less than 0.01m.Then next index point correction is carried out.

It is an advantage of the invention that and propose a kind of pipeline survey field inertial navigation odometer Combinated navigation method, the method Recycle global test data, by forward and reverse associating Navigation, every error of inertial navigation system and odometer is estimated Meter and compensation, finally combine index point positional information and further revised track, the high-precision pipeline rail of final acquisition Mark data.

Accompanying drawing explanation

Fig. 1 is a kind of pipeline mapping inertial navigation odometer Combinated navigation method flow chart provided by the present invention.

Detailed description of the invention

With specific embodiment, the present invention is described in detail below in conjunction with the accompanying drawings:

Positional information first with initial point initially sets, and then carries out the navigation calculation of forward, dead reckoning And Kalman filter, in whole filtering, three velocity errors, three misalignments are carried out Closed-cycle correction, when calculating extremely Time at the end of data, proceed reverse navigation calculation, dead reckoning and Kalman filter, complete the most complete just To and after reversely combining navigation and filtering, following error is once revised, including: inertial navigation system gyroscopic drift, add Speedometer zero partially, odometer site error, odometer fix error angle and the scale coefficient error of odometer, then proceed to into The navigation of row forward and filtering, and preserve the numbers such as inertial navigation system attitude matrix, odometer displacement increment and integrated navigation result According to, finally, the track obtained in conjunction with the navigation of index point data pair combinations is further revised.

Initially set up the error model of inertial navigation system and odometer, then utilize the omnidistance data of pipe detection to carry out order Positive navigation and dead reckoning, carry out Kalman filter simultaneously and estimate each error term, when calculating to after the end of data, carry out anti- Navigation and dead reckoning, Kalman filter continues estimation error, and period wave filter is not restarted, until data start position, Then carry out compensation and the correction of each error term, then carry out navigation and the filtering of forward, now all carry out due to each main error Compensate, the most high-precision track data will be obtained, finally may also be combined with index point information the track of gained is entered The correction of one step.

A kind of pipeline mapping inertial navigation odometer Combinated navigation method, it comprises the steps:

1. error model

Inertial navigation odometer Integrated Navigation Algorithm uses " speed+position " coupling Kalman filter scheme, and the program uses 19 Rank navigation error model, choosing 19 error state variable is

$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]$

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_eFor inertial navigation north orientation, vertical, east orientation velocity error；Φ_n、Φ_u、Φ_eFor inertial navigation north orientation, vertical, East orientation misalignment；Inertial navigation X, Y, Z axis accelerometer bias；ε_x、ε_y、ε_z: inertial navigation X, Y, Z axis gyroscopic drift； δL_D、δh_D、δλ_DFor odometer dead reckoning latitude, highly, longitude error；Θ_Y、Θ_ZFor between inertial navigation and odometer along inertial navigation Y Fix error angle with Z axis；Δ SF: odometer scale coefficient error；Δ t: for time delay；f_n、f_u、f_eSurvey for inertial navigation system The northern sky east tripartite of amount specific force upwards；V_n、V_u、V_eIt is respectively inertial navigation system north orientation, vertical and east orientation speed；ω_ieFor the earth certainly Corner speed；L represents the latitude of inertial navigation system present position；R_N、R_MIt is respectively radius of curvature in prime vertical, meridian plane incurvature half Footpath, V_DForward speed for odometer；V_Dn、V_Du、V_DeRepresent the north orientation of odometer dead reckoning, vertical, east orientation speed respectively；Representing matrixThe element that 1st row the 1st row are corresponding, the implication of remaining same form variable is identical with this.

2) observational equation

Wave filter observational equation is divided into two parts, and when there being speed to observe, observational 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 When having reference point information, observational 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}$

The accekeration of the inertial navigation system Department of Geography updated for speed when representing navigation calculation respectively； Remaining each variable-definition ibid saves.

3) equation discretization

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 cycle, T_n=0.005s, T_eFor filtering cycle, T_e=1s。A_tFor the state-transition matrix of t, φ_k+1,kFor the transfer matrix after discretization, t=0 when each filtering cycle starts.

Noise battle array discretization formula is:

Q_k=QT_e

Q is noise battle array.

Observed quantity solution 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, λ_D、L_D、h_DFor the longitude of odometer dead reckoning, latitude, highly；λ_M、L_M、h_MFor the longitude at index point, Latitude, highly.The speed of odometer is obtained by following formula:

$\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 formula, Δ S is the displacement increment in 0.1s, dT=0.1s.

2. dead reckoning

Dead reckoning is carried out while carrying out inertial navigation.The calculating cycle is with navigation calculation cycle, Tn=5 (ms)；Boat position pushes away Calculate positional information and initialize same navigation calculation, i.e. utilize extraneous bookbinding value to obtain initial longitude, latitude and height.

If the displacement that odometer exports carrier in the i-th sampling period is Δ S_i, then it is in inertial navigation carrier coordinate system b system It is expressed as:

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

The attitude matrix utilizing inertial navigation system converts it under geographic coordinate system:

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

For eliminating odometer quantizing noise, for the cycle, speed is made smoothing processing with 0.1s.

Dead reckoning formula is:

$\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)$

The displacement increment of odometer Department of Geography in the expression navigation calculation cycle respectively.

3.Kalman Filtering Model

Kalman filter equation uses document " Kalman filtering and integrated navigation principle " (first edition, Qin Yongyuan etc. writes) In form, concrete formula is as follows:

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)$

Filtering 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 covariance 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 battle array

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

Wherein,It is a step status predication value,For state estimation matrix, Φ_k,k-1For state Matrix of shifting of a step, H_kFor measurement matrix, Z_kFor measurement, K_kFor filtering gain matrix, R_kFor observation noise battle array, P_k,k-1For one-step prediction error variance Battle array, P_kFor estimation error variance battle array, Γ_k,k-1Battle array, Q is driven for system noise_k-1For system noise acoustic matrix.

4. reverse process

After the navigation completing forward sequence and filtering, navigation calculation, dead reckoning and Kalman filter do not stop, But proceed reverse navigation calculation, dead reckoning and Kalman filter, main methods includes:

1) reverse navigation

The computing formula of navigation, with positive navigation, is a difference in that during calculating and needs to take some quantity of states Instead, concrete process includes:

H) acceleration of gravity is reverse: g=-g

I) overload is reversely: fb=-fb

J) angular speed is reverse: wibb=-wibb

K) rotational-angular velocity of the earth is reverse: wien=-wien

L) angular speed is involved reverse: wenn=-wenn

M) location updating is reverse: speed negates

N) time: t=t-Tn

2) reversely dead reckoning

Reversely the form of dead reckoning is with the dead reckoning of forward, and the difference with forward dead reckoning is when displacement adds up Take negative sign, i.e. formula becomes:

$\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

Inverse filtering flow process calculates with forward filtering, only need to be negated i.e. by all elements in state matrix and measurement matrix Can.

5. index point correction

After utilizing forward and reverse Federated filter to complete the estimation to error term and compensate, i.e. can get relatively accurate rail Mark information, but owing to the remainder error of odometer still has additive, therefore typically require and by index point, track is entered Row is further revised, and concrete method is as follows.

The error of calculating odometer dead reckoning at index point:

$\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_DFor the longitude of odometer dead reckoning, latitude, highly；λ_M、L_M、h_MFor the longitude at index point, Latitude, highly.

Then can obtain

$\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、ΔΦ_L, Δ SF be respectively odometer remaining course fix error angle, pitching fix error angle with And scale coefficient error；S_Dn、S_Du、S_De、S_DLIt is respectively north orientation, vertical, east orientation and radial direction position that odometer dead reckoning obtains Move；

After the error parameter estimated, make the following judgment, when radial error is more than 0.01m, i.e.

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

Time, carry out adding up and compensating to calculating the odometer remainder error obtained, and again enter at a upper index point Row calculates, and the compensation method of odometer remainder error is as follows:

First error is added up:

$\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)$

Then error is compensated:

$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)

So it is iterated calculating between two index points, until the radial position error at this index point is less than 0.01m.Then next index point correction is carried out.

Claims (1)

1. a pipeline mapping inertial navigation odometer Combinated navigation method, it is characterised in that: it comprises the steps,

(1) computation model is set up,

(2) boat position calculates,

(3) Kalman filter model,

(4) reverse process,

(5) index point correction；

Described step (1) includes,

1) error model

Inertial navigation odometer Integrated Navigation Algorithm uses " speed+position " coupling Kalman filter method, and the program uses 19 rank to lead Boat error model, choosing 19 error state variable is

$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]$

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_eFor inertial navigation north orientation, vertical, east orientation velocity error；Φ_n、Φ_u、Φ_eFor inertial navigation north orientation, vertical, eastern To misalignment；Inertial navigation X, Y, Z axis accelerometer bias；ε_x、ε_y、ε_z: inertial navigation X, Y, Z axis gyroscopic drift；δ L_D、δh_D、δλ_DFor odometer dead reckoning latitude, highly, longitude error；Θ_Y、Θ_ZFor between inertial navigation and odometer along inertial navigation Y and The fix error angle of Z axis；Δ SF: odometer scale coefficient error；Δ t: for time delay；f_n、f_u、f_eMeasure for inertial navigation system Northern sky east tripartite specific force upwards；V_n、V_u、V_eIt is respectively inertial navigation system north orientation, vertical and east orientation speed；ω_ieFor earth rotation Angular speed；L represents the latitude of inertial navigation system present position；R_N、R_MIt is respectively radius of curvature in prime vertical, meridian plane incurvature radius, V_DForward speed for odometer；V_Dn、V_Du、V_DeRepresent the north orientation of odometer dead reckoning, vertical, east orientation speed respectively；Representing matrixThe element that 1st row the 1st row are corresponding, the implication of remaining same form variable is identical with this；

2) observational equation

Wave filter observational equation is divided into two parts, and when there being speed to observe, observational 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 there being reference point information, observational 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$

The accekeration of the inertial navigation system Department of Geography updated for speed when representing navigation calculation respectively；Remaining is each Variable-definition ibid saves；

3) equation discretization

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 cycle, T_n=0.005s, T_eFor filtering cycle, T_e=1s, A_tFor the state-transition matrix of t, φ_k+1,kFor the transfer matrix after discretization, t=0 when each filtering cycle starts；

Noise battle array discretization formula is:

Q_k=QT_e

Q is noise battle array,

Observed quantity solution 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, λ_D、L_D、h_DFor the longitude of odometer dead reckoning, latitude, highly；λ_M、L_M、h_MFor the longitude at index point, latitude Degree, highly, the speed of odometer is obtained by following formula:

$\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]$

In formula, Δ S is the displacement increment in 0.1s, dT=0.1s；

Described step (2) includes,

Carrying out dead reckoning while carrying out inertial navigation, the cycle that calculates is with navigation calculation cycle, Tn=5 (ms)；Dead reckoning Positional information initializes same navigation calculation, i.e. utilizes extraneous bookbinding value to obtain initial longitude, latitude and height,

If the displacement that odometer exports carrier in the i-th sampling period is Δ S_i, then its expression in inertial navigation carrier coordinate system b system For:

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

The attitude matrix utilizing inertial navigation system converts it under geographic coordinate system:

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

For eliminating odometer quantizing noise, for the cycle, speed is made smoothing processing with 0.1s,

Dead reckoning formula is:

$\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.$

The displacement increment of odometer Department of Geography in the expression navigation calculation cycle respectively；

Described step (3) includes, the formula of Kalman filter equation is as follows:

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}\]$

Filtering 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 covariance 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 battle array

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

Wherein,It is a step status predication value,For state estimation matrix, Φ_k,k-1For state Matrix of shifting of a step, H_kFor Measurement matrix, Z_kFor measurement, K_kFor filtering gain matrix, R_kFor observation noise battle array, P_k,k-1For one-step prediction error covariance matrix, P_kFor estimation error variance battle array, Γ_k,k-1Battle array, Q is driven for system noise_k-1For system noise acoustic matrix；

Described step (4) includes,

After the navigation completing forward sequence and filtering, navigation calculation, dead reckoning and Kalman filter do not stop, but Proceeding reverse navigation calculation, dead reckoning and Kalman filter, main methods includes:

1) reverse navigation

The computing formula of navigation, with positive navigation, is a difference in that during calculating and needs to negate some quantity of states, Concrete process includes:

A) acceleration of gravity is reverse: g=-g

B) overload is reversely: fb=-fb

C) angular speed is reverse: wibb=-wibb

D) rotational-angular velocity of the earth is reverse: wien=-wien

E) angular speed is involved reverse: wenn=-wenn

F) location updating is reverse: speed negates

G) time: t=t-Tn

2) reversely dead reckoning

Reversely the form of dead reckoning is with the dead reckoning of forward, the difference with forward dead reckoning be take when displacement adds up negative Number, i.e. formula becomes:

$\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

Inverse filtering flow process calculates with forward filtering, only need to be negated by all elements in state matrix and measurement matrix；

Described step (5) includes, after utilizing forward and reverse Federated filter to complete the estimation to error term and compensate, To relatively accurate trace information, but owing to the remainder error of odometer still has additive, therefore typically require and pass through Track is further revised by index point, and concrete method is as follows:

The error of calculating odometer dead reckoning at index point:

$\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_DFor the longitude of odometer dead reckoning, latitude, highly；λ_M、L_M、h_MFor the longitude at index point, latitude, Highly,

Then can obtain

$\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、ΔΦ_L, Δ SF be respectively odometer remaining course fix error angle, pitching fix error angle and quarter Degree system errors；S_Dn、S_Du、S_De、S_DLIt is respectively north orientation, vertical, east orientation and radial displacement that odometer dead reckoning obtains；

After the error parameter estimated, make the following judgment, when radial error is more than 0.01m, i.e.

dS_D=sqrt (dL_D*dL_D+dh_D*dh_D+dλ_D*dλ_D) > 0.01m time, to calculate obtain odometer remainder error tire out Adding and compensate, and re-starting calculating at a upper index point, the compensation method of odometer remainder error is as follows:

First error is added up:

$\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]$

Then error is compensated:

${\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

So it is iterated calculating between two index points, until the radial position error at this index point is less than 0.01m, so After carry out next index point correction.