CN106323226A - System for measuring inertial navigation by using big dipper and method for installing included angle by tachymeter - Google Patents

Abstract

The invention relates to a system for measuring inertial navigation by using a big dipper and a method for installing an included angle by a tachymeter, which are suitable for a combination navigation system formed by a big dipper receiver, an inertial navigation system and the tachymeter, and belong to the technical field of combination navigation. The technical key points are characterized in that 1) establishing a relation model of installation included angle and the tachymeter path increment error, accurately modeling an observation matrix; 2) according to the speed output of the big dipper receiver, calculating the carrier position increment which is used for constructing the observed value of the combination navigation system; and 3) employing a closed loop feedback updating mode, and realizing the rapid and accurate estimation of the inertial navigation system and the tachymeter installation included angle. By combining the big dipper receiver and the inertial navigation system information, compared with the single usage of the big dipper receiver or the inertial navigation system, the output information with higher precision can be obtained, so that the measurement of the installation included angle can be rapidly and accurately measured.

Description

A kind of method utilizing the Big Dipper to measure inertial navigation system and tachymeter installation angle

Technical field

The present invention relates to a kind of method utilizing the Big Dipper to measure inertial navigation system and tachymeter installation angle, it is adaptable to connect by the Big Dipper The integrated navigation system that receipts machine, inertial navigation system and tachymeter are constituted, belongs to integrated navigation technology field.

Background technology

Inertial navigation system utilizes Newton's second law, in the feelings being independent of the information such as extraneous light, electromagnetic wave, sound wave and magnetic field Under condition, moved by the angular movement and line that the measurement of gravitational field and inertia force is come perceptual object, it is achieved the navigation to carrier. Its advantage include output information comprehensively, work strong etc. from external interference resistance of advocating peace, obtain in land, sea, air, field, sky Extensive application.Being disadvantageous in that inertial navigation system is substantially a kind of dead reckoning system, navigation error is the most continuous Accumulation.

For improving the navigation accuracy that inertial navigation system works long hours, the general mode using integrated navigation realizes inertial navigation system The error correction of system.In land navigation field, along with becoming better and approaching perfection day by day of triones navigation system, by Beidou receiver and inertial navigation The application of the onboard combined navigation system that system is constituted is more and more extensive.The Big Dipper/inertia combined navigation system connects at Big Dipper satellite signal Receive good under conditions of, it is possible to realize high accuracy navigation, but when vehicle travel in mountain area, the environment such as tunnel, urban canyons time, Owing to Big Dipper signal is blocked, the precision of integrated navigation system will inevitably be affected.Therefore, it is necessary to the Big Dipper/ On the basis of inertia combined navigation system, tachymeter is brought in integrated navigation system, constitute the Big Dipper/inertia/tachymeter combination and lead Boat system.In the case of Beidou receiver normally works, use the Big Dipper/inertia/tachymeter Integrated navigation mode, at big-dipper satellite In the case of signal is blocked, it is switched to inertia/tachymeter Integrated navigation mode.

Owing to inertial navigation system and tachymeter are separately mounted to the different parts of car body, cause the body coordinate system of inertial navigation system Certain angle is there is with the body coordinate system of tachymeter.The load measured for the bearer rate and tachymeter that ensure inertial navigation system measurement Body speed has common reference, needs to estimate installation angle.

The main method calculating inertial navigation system and tachymeter installation angle at present includes: (1) utilizes two accurate known roads Punctuate, installs angle by sport car experimental calculation；(2) GPS is utilized to realize angle estimation is installed with tachymeter combination；(3) profit Realize angle estimation is installed with tachymeter combination by inertial navigation system.

The deficiency of first method is to need road sign point to assist, and limits the range of application of integrated navigation system, and calculated Installing angle is fixed value, needs when angle changes to redeterminate；The deficiency of second method is to carry out installing angle During estimation, gps signal can not be blocked, and vehicle running environment proposes higher requirement, and the time of estimation is longer；The third Method requires that inertial navigation system has the highest precision, it is difficult in being used in low accuracy inertial navigation system.

Summary of the invention

It is an object of the invention to the deficiency overcoming prior art to exist, it is provided that one utilizes the Big Dipper to measure inertial navigation system and test the speed The method of angle installed by instrument.The method, by merging Beidou receiver and the information of inertial navigation system, obtains than each separate payment Precision higher output information, and then realize inertial navigation system and tachymeter installation angle mensuration by modeling, reduce and measured Requirement to vehicle running environment in journey, shortens minute, and is applicable to the inertial navigation system of all kinds of precision.

A kind of method utilizing the Big Dipper to measure inertial navigation system and tachymeter installation angle, is arranged on load car by inertial navigation system In compartment, Beidou receiver is arranged on roof, and tachymeter is connected with driving wheel bearing, at the beginning of inertial navigation system completes by flexible axle Setting in motion after beginning to be directed at, its abscissa is latitude, and vertical coordinate is longitude；3 gyroscopes that inertial navigation system comprises with Machine drift is 0.01 °/h, and constant value drift is 0.02 °/h；3 accelerometer random drifts that inertial navigation system comprises are equal Being 50 μ g, constant value drift is 100 μ g, and the initial calibration factor of tachymeter is 0.01 meter/pulse, including step:

Step one, setting up inertial navigation system dynamic error model, state variable includes site error, velocity error, misalignment With inertia device drift error:

Northeastward under sky coordinate system, inertial navigation system dynamic error model is:

In formula, t is time value, is arithmetic number；x_INS(t) table Show the state vector of inertial navigation system dynamic error model, by site error δ P, velocity error δ Vⁿ, misalignmentGyro Instrument zero offset error δ ε_gWith accelerometer bias error delta V_aComposition；w_INST () represents the system of inertial navigation system dynamic error model Noise；F_INST () is inertial navigation system dynamic error transfer matrix；Represent inertial navigation system dynamic error model shape The derivative of state vector；

Inertial navigation system dynamic error model is carried out sliding-model control, obtains:

x_INS(k)=F_INS(k, k-1) x_INS(k-1)+w_INS(k)

In formula, k represents the moment, and for positive integer, the time interval in k-1 moment to k moment is designated as Δ T_Dis, represent error model The discretization cycle；F_INS(k, k-1) represents the inertial navigation system dynamic error transfer matrix in k-1 moment to k moment； x_INSK () is the state vector of the k moment inertial navigation system after discretization；w_INSK () represents that the inertial navigation system in k moment is moved The system noise of state error model, be average be zero, variance is Q_INSWhite noise sequence, Q_INSFor nonnegative matrix；

Step 2, setting up tachymeter dynamic error model, the state variable that this model comprises is for installing angle error and and tachymeter Scale factor error, wherein installs angle error and includes that course is installed angle error and installed angle error with pitching,

Tachymeter dynamic error model is: ${\stackrel{.}{x}}_{\mathrm{VMS}}\left(t\right)=F_{\mathrm{VMS}}\left(t\right)x_{\mathrm{VMS}}\left(t\right)+w_{\mathrm{VMS}}\left(t\right)$

In formula, x_VMS(t)=[δ α δ β δ τ]^T, x_VMST () represents the state vector of tachymeter dynamic error model, by course Install angle error δ α, angle error δ β and tachymeter scale factor error δ τ composition is installed in pitching；w_VMST () represents tachymeter The system noise of dynamic error model；F_VMST () is tachymeter dynamic error transfer matrix；Represent that tachymeter dynamically misses The derivative of differential mode type state vector；

Tachymeter dynamic error model is carried out sliding-model control, obtains:

x_VMS(k)=F_VMS(k, k-1) x_VMS(k-1)+w_VMS(k)

In formula, F_VMS(k, k-1) represents the tachymeter dynamic error transfer matrix in k-1 moment to k moment；x_VMS(k) be from The state vector of the k moment tachymeter dynamic error model after dispersion；w_VMSK () represents the tachymeter dynamic error model in k moment System noise, be average be zero, variance is Q_VMSWhite noise sequence, Q_VMSFor nonnegative matrix；

The inertial navigation system dynamic error model that step 3, combining step one are set up, and the tachymeter that step 2 is set up is dynamic Error model, structure integrated navigation system dynamic error model:

In formula, $x\left(t\right)=\left[\begin{array}{c}{x}_{\mathrm{INS}}\left(t\right)\\ x_{\mathrm{VMS}}\left(t\right)\end{array}\right]$ Represent the state vector of integrated navigation system dynamic error model, inertial navigation system move State vector x of state error model_INSState vector x of (t) and tachymeter dynamic error model_VMST () forms； $w\left(t\right)=\left[\begin{array}{c}{w}_{\mathrm{INS}}\left(t\right)\\ w_{\mathrm{VMS}}\left(t\right)\end{array}\right]$ Represent the system noise of integrated navigation system dynamic error model, by inertial navigation system dynamic error mould System noise w of type_INSSystem noise w of (t) and tachymeter dynamic error model_VMST () forms； $F\left(t\right)=\left[\begin{array}{cc}{F}_{\mathrm{INS}}\left(t\right)& O_{15\×3}\\ O_{3\×15}& F_{\mathrm{VMS}}\left(t\right)\end{array}\right]$ For integrated navigation system dynamic error transfer matrix, O_15×3Represent the null matrix of 15 row 3 row； O_3×15Represent the null matrix of 3 row 15 row；Represent the derivative of integrated navigation system dynamic error model state vector；

Integrated navigation system dynamic error model is carried out sliding-model control, obtains:

X (k)=F (k, k-1) x (k-1)+w (k)

In formula, F (k, k-1) represent k-1 moment to the k moment integrated navigation system dynamic error transfer matrix, x (k) be from The state vector of the k moment integrated navigation system dynamic error model after dispersion；W (k) represents that the integrated navigation system in k moment is dynamic The system noise of error model, be average be zero, variance is the white noise sequence of Q, and Q-value is according to Q_INSAnd Q_VMSDetermine；

Step 4, the positional increment output of calculating inertial navigation system, the positional increment output of Beidou receiver and the position of tachymeter Putting increment output, detailed process is:

(I) output of inertial navigation system positional increment is calculated

According to the speed that inertial navigation system is given, calculate the positional increment in j-1 moment to j momentComputing formula is:

${\mathrm{\ΔR}}_j^{\mathrm{INS}}={\mathrm{\Σ}}_{r=(j-1)\×N_1+1}^{j\×N_1}{V}_r^n\×{\mathrm{\ΔT}}_{\mathrm{INS}}$

Wherein, j represents the moment, and for positive integer, the time interval in j-1 moment to j moment is designated as Δ T_Kal, represent Kalman filtering In the cycle, determine according to carrier running environment and integrated navigation system required precision；ΔT_INSRepresent inertial navigation system speed output week Phase；R represents that inertial navigation system exports the sequential value of result in the j-1 moment to the j moment；Represent inertial navigation system output Sequential value is the speed of r；N₁=Δ T_Kal/ΔT_INS；

(II) output of Beidou receiver positional increment is calculated

According to the speed that Beidou receiver is given, consider the lever arm effect between Beidou receiver and inertial navigation system, meter simultaneously Calculate the positional increment in j-1 moment to j momentComputing formula is:

${\mathrm{\ΔR}}_j^{\mathrm{BD}}={\mathrm{\Σ}}_{q=(j-1)\×N_2+1}^{j\×N_z}{V}_q^{\mathrm{BD}}\×{\mathrm{\ΔT}}_{\mathrm{BD}}+C_j({\mathrm{\ω}}_j\×L^{\mathrm{BD}})\×\mathrm{\Δ}{T}_{\mathrm{Kal}}$

Wherein, Δ T_BDRepresent the Beidou receiver speed output cycle；Q represents that Beidou receiver exports knot in the j-1 moment to the j moment The sequential value of fruit；Represent the speed that sequential value is q of Beidou receiver output；N₂=Δ T_Kal/ΔT_BD；C_jRepresent inertia The attitude matrix that navigation system exported in the j moment, ω_jRepresent the angular velocity that inertial navigation system exports, L in the j moment^BDRepresent by north Bucket receiver antenna is vectorial to the lever arm of inertial navigation system casing center；

(III) output of tachymeter positional increment is calculated

1. the tachymeter scale factor error δ τ in the j-1 moment that step 7 estimates is utilized_j-1To tachymeter in the j-1 moment to the j moment Distance increment be modified, i.e.δτ_j-1Initial value is set to zero, s_jIncrease for revised distance Amount,For revising the front distance increment gathered,

Angle α is installed in the course in the j-1 moment 2. updated according to step 8_j-1With pitching, angle β is installed_j-1, build direction cosines Vector M_j-1, M_j-1It is the distance increment s that tachymeter is recorded_jProject in inertial navigation system body coordinate system, α_j-1And β_j-1's Initial value is set to zero,

$M_{j-1}=\left[\begin{array}{c}-\sin \left({\mathrm{\α}}_{j-1}\right)\cos \left({\mathrm{\β}}_{j-1}\right)\\ \cos \left({\mathrm{\α}}_{j-1}\right)\cos \left({\mathrm{\β}}_{j-1}\right)\\ \sin \left({\mathrm{\β}}_{j-1}\right)\end{array}\right]$

3. by revised tachymeter distance increment s_jProject in sky, northeast coordinate system, and consider tachymeter and inertial navigation system Between lever arm effect, calculate tachymeter in the positional increment in j-1 moment to j momentComputing formula is:

$\mathrm{\Δ}{R}_j^{\mathrm{VMS}}=C_j{M}_{j-1}{s}_j+C_j({\mathrm{\ω}}_j\×L^{\mathrm{VMS}})\×\mathrm{\Δ}{T}_{\mathrm{Kal}}$

Wherein, L^VMSRepresent by the lever arm vector of tachymeter to inertial navigation system casing center, acquisition can be measured in advance；

Step 5, the inertial navigation system positional increment, Beidou receiver positional increment and the increasing of tachymeter position that calculate with step 4 Based on amount, build dynamic error model observation Z_j, computing formula is:

$Z_j=\left[\begin{array}{c}{\mathrm{\ΔR}}_j^{\mathrm{INS}}-{\mathrm{\ΔR}}_j^{\mathrm{VMS}}\\ \mathrm{\Δ}{R}_j^{\mathrm{INS}}-{\mathrm{\ΔR}}_j^{\mathrm{BD}}\end{array}\right]$

Step 6, according to the formula Z in step 5_j, the observation of the integrated navigation system dynamic error model described in establishment step three Equation: Y_j=H_jx_j+ρ_j

Wherein, Y_jRepresent the observation sequence in j moment；ρ_jRepresent the observational equation of the integrated navigation system dynamic error model in j moment Noise, be average be zero, variance is the white noise sequence of R, and R value is manually set according to actual application environment, and R is nonnegative matrix； H_jFor the observing matrix in j moment, formula is as follows:

$H_j=\left[\begin{array}{c}{O}_{3\×3}{I}_{3\×3}-\left(C_j{M}_{j-1}{s}_j\right)\×O_{3\×6}-C_j{U}_{j-1}{s}_j-C_j{M}_{j-1}{s}_j\\ O_{3\×3}\mathrm{\Δ}{T}_{\mathrm{Kal}}\×I_{3\×3}{O}_{3\×12}\end{array}\right]$

Wherein, O_3×3Represent 3 rank null matrix；O_3×6Represent 3 row 6 row null matrix；O_3×12Represent 3 row 12 row null matrix；I_3×3 Represent 3 rank unit matrix；(C_jM_j-1s_j) × represent by C_jM_j-1s_jThe most negative symmetrical matrix constituted；U_j-1Updated by step 8 Angle α is installed in the course in j-1 moment_j-1With pitching, angle β is installed_j-1Formed, α_j-1And β_j-1Initial value be set to zero:

$U_{j-1}=\left[\begin{array}{cc}-\cos \left({\mathrm{\α}}_{j-1}\right)\cos \left({\mathrm{\β}}_{j-1}\right)& \sin \left({\mathrm{\α}}_{j-1}\right)\sin \left({\mathrm{\β}}_{j-1}\right)\\ -\sin \left({\mathrm{\α}}_{j-1}\right)\cos \left({\mathrm{\β}}_{j-1}\right)& -\cos \left({\mathrm{\α}}_{j-1}\right)\sin \left({\mathrm{\β}}_{j-1}\right)\\ 0& \cos \left({\mathrm{\β}}_{j-1}\right)\end{array}\right]$

Wherein, there are a following relation in j moment and k moment:

N×(t_k-t_k-1)=t_j-t_j-1

Wherein, N is positive integer；t_k-t_k-1Represent the time interval in k-1 moment to k moment；t_j-t_j-1Represent the j-1 moment Time interval to the j moment；

Observational equation in step 7, the integrated navigation system dynamic error model set up according to step 3 and step 6, in conjunction with step Rapid five observations be given, use Kalman filter to estimate state vector x (k) of integrated navigation system dynamic error model Meter:

Kalman filter is classic card Thalmann filter, calculates process as follows:

$\hat{x}(k,k-1)=F(k,k-1)\hat{x}(k-1)$

P (k, k-1)=F (k, k-1) P (k-1) [F (k, k-1)]^T+Q

$K\left(k\right)=P(k,k-1)H_k^T{[H_{k}P(k,k-1)H_k^T+R]}^{-1}$

$\hat{x}\left(k\right)=\hat{x}(k,k-1)+K\left(k\right)[Z_k-H_k\hat{x}(k,k-1)]$

P (k)=P (k, k-1)-K (k) H_kP (k, k-1)

Wherein,Represent the one-step prediction of state vector x (k) of integrated navigation system dynamic error model；Table Show the estimated value of state vector x (k) of integrated navigation system dynamic error model；P (k, k-1) represents one-step prediction variance；P(k) Represent estimate variance；K (k) represents filtering gain；

If k ≠ j, the most only carry out state recursion, even the null matrix that filtering gain K (k) is equal dimension；If k=j, then enter The above-mentioned complete computation of row；

The estimated value of k moment integrated navigation system dynamic error model state vector x (k) is i.e. can get through above-mentioned stepsBag Containing site error (δ P)_k, velocity error (δ Vⁿ)_k, misalignmentGyroscope zero offset error (δ ε_g)_k, accelerometer bias by mistake DifferenceAngle error (δ α) is installed in course_k, pitching install angle error (δ β)_kWith tachymeter scale factor error (δ τ)_k；

Step 8, estimated result according to step 7 carry out error correction to integrated navigation system, specifically refer to utilize step 7 to obtain The position of k moment inertial navigation system is exported by the error estimation result takenSpeed exportsC is exported with attitude matrix_kEnter Row correction, ε inclined to gyroscope zero simultaneously_g, accelerometer biasAngle α is installed in course, angle β and tachymeter are installed in pitching Calibration factor τ is updated, and computing formula is:

$P_k^{\mathrm{Cor}}=P_k^{\mathrm{INS}}-{\left(\mathrm{\δP}\right)}_k$

$V_k^{\mathrm{Cor}}=V_k^n-{\left({\mathrm{\δV}}^n\right)}_k$

(ε_g)_k=(ε_g)_k-1-(δε_g)_k

${\left({\▿}_a\right)}_k={\left({\▿}_a\right)}_{k-1}-{\left({\mathrm{\δ}\▿}_a\right)}_k$

α_k=α_k-1-(δα)_k

β_k=β_k-1-(δβ)_k

τ_k=[1-(δ τ)_k]×τ_k-1

Wherein,WithRepresent position, speed and the attitude matrix after correction respectively；× represent byStructure The most negative symmetrical matrix become；(ε_g)_kWithRepresenting the gyroscope zero in k moment respectively partially and accelerometer bias, initial value is Three-dimensional null vector；α_k、β_kAnd τ_kRepresent that angle, pitching installation angle and tachymeter calibration factor are installed in the course in k moment respectively, Initial value is disposed as zero；

When inertial navigation system carries out subsequent time navigation calculation, utilize (ε_g)_kWithTo the angular velocity gathered and specific force Compensate, and utilize α_k、β_kAnd τ_kVariable corresponding in step 4 and step 6 is updated；

Step 9, integrated navigation system dynamic error model Matrix of shifting of a step F (k, k-1) in step one, two and three is entered Row updates, and the value of k+1 is assigned to k, and arranging system mode is 18 dimension null vectors, then returnes to step 4.

Compared with prior art, the invention have the advantage that (1) by merging Beidou receiver and the information of inertial navigation system, Ratio can be obtained and be used alone Beidou receiver or inertial navigation system precision higher output information, thus more quickly with accurate Ground realizes installing the mensuration of angle；(2) inertial navigation system has the effect of low-pass filtering, is used in combination with Beidou receiver, Beidou receiver can be overcome to export when there is outlier the impact installing angle measurement accuracy, relax installation angle and measure process In requirement to carrier running environment；(3) Beidou receiver have degree of precision speed output, even if with lower accuracy Inertial navigation system is applied in combination, it is possible to realizes high accuracy navigation information output, and then realizes inertial navigation system and tachymeter peace The mensuration at clamping angle, relaxes the required precision of inertial navigation system.

Accompanying drawing explanation

Fig. 1 is the load car running orbit schematic diagram in the specific embodiment of the present invention.

Fig. 2 is that angle estimation schematic diagram is installed in the course in the specific embodiment of the present invention.

Fig. 3 is that angle estimation schematic diagram is installed in the pitching in the specific embodiment of the present invention.

Detailed description of the invention

The key problem in technology point of the present invention is: 1. set up the relational model installing angle error with tachymeter distance incremental error, right Observing matrix carries out Accurate Model；2. export according to the speed of Beidou receiver and carry out carrier positional increment calculating, be used for building The observation of integrated navigation system；3. the mode that closed loop feedback updates is used, it is achieved inertial navigation system installs angle with tachymeter Quick and precisely estimation.

A kind of method utilizing the Big Dipper to measure inertial navigation system and tachymeter installation angle, is arranged on load car by inertial navigation system In compartment, Beidou receiver is arranged on roof, and tachymeter is connected with driving wheel bearing, at the beginning of inertial navigation system completes by flexible axle Begin setting in motion after alignment, and its abscissa is latitude, and vertical coordinate is longitude, 3 gyroscopes that inertial navigation system comprises random Drift is 0.01 °/h, and constant value drift is 0.02 °/h；3 accelerometer random drifts that inertial navigation system comprises are 50 μ g, constant value drift is 100 μ g, and the initial calibration factor of tachymeter is 0.01 meter/pulse, comprises the following steps:

Step one, setting up inertial navigation system dynamic error model, this model is Φ angle error equation or Ψ angle error equation, shape State includes site error, velocity error, misalignment and inertia device drift error.

Northeastward under sky coordinate system, inertial navigation system dynamic error model represents as shown in formula (1):

${\stackrel{\·}{x}}_{\mathrm{INS}}\left(t\right)=F_{\mathrm{INS}}\left(t\right)x_{\mathrm{INS}}\left(t\right)+w_{\mathrm{INS}}\left(t\right)---\left(1\right)$

In formula, t is time value, is arithmetic number； Represent the state vector of inertial navigation system dynamic error model, by site error δ P, velocity error δ Vⁿ, misalignmentTop Spiral shell instrument zero offset error δ ε_gWith accelerometer bias errorComposition；w_INS(t) represent inertial navigation system dynamic error model be System noise；F_INST () is inertial navigation system dynamic error transfer matrix；Represent inertial navigation system dynamic error model The derivative of state vector；

Inertial navigation system dynamic error model is carried out sliding-model control, can obtain:

x_INS(k)=F_INS(k, k-1) x_INS(k-1)+w_INS(k) (2)

In formula, k represents the moment, and for positive integer, the time interval in k-1 moment to k moment is designated as Δ T_Dis, represent error model In the discretization cycle, belong to and be manually set, be typically based on carrier running environment and integrated navigation system required precision is determined； F_INS(k, k-1) represents the inertial navigation system dynamic error transfer matrix in k-1 moment to k moment；x_INSK () is discretization after The state vector of k moment inertial navigation system；w_INSK () represents the system noise of the inertial navigation system dynamic error model in k moment Sound, be average be zero, variance is Q_INSWhite noise sequence, Q_INSValue is manually set according to actual application environment, Q_INSFor non- Negative matrix.

Step 2, setting up tachymeter dynamic error model, the state that this model comprises (includes that course is installed for installing angle error Angle error installs angle error with pitching) and tachymeter scale factor error.

Tachymeter dynamic error model represents as shown in formula (3):

${\stackrel{\·}{x}}_{\mathrm{VMS}}\left(t\right)=F_{\mathrm{VMS}}\left(t\right)x_{\mathrm{VMS}}\left(t\right)+w_{\mathrm{VMS}}\left(t\right)---\left(3\right)$

In formula, x_VMS(t)=[δ α δ β δ τ]^T, x_VMST () represents the state vector of tachymeter dynamic error model, by course Install angle error δ α, angle error δ β and tachymeter scale factor error δ τ composition is installed in pitching；w_VMST () represents tachymeter The system noise of dynamic error model；F_VMST () is tachymeter dynamic error transfer matrix；Represent that tachymeter dynamically misses The derivative of differential mode type state vector.

Tachymeter dynamic error model is carried out sliding-model control, can obtain:

x_VMS(k)=F_VMS(k, k-1) x_VMS(k-1)+w_VMS(k) (4)

In formula, F_VMS(k, k-1) represents the tachymeter dynamic error transfer matrix in k-1 moment to k moment；x_VMS(k) be from The state vector of the k moment tachymeter dynamic error model after dispersion；w_VMSK () represents the tachymeter dynamic error model in k moment System noise, be average be zero, variance is Q_VMSWhite noise sequence, Q_VMSValue is manually set according to actual application environment, Q_VMSFor nonnegative matrix.

The inertial navigation system dynamic error model that step 3, combining step one are set up, and the tachymeter that step 2 is set up is dynamic Error model, constitutes following integrated navigation system dynamic error model:

$\stackrel{\·}{x}\left(t\right)=F\left(t\right)x\left(t\right)+w\left(t\right)---\left(5\right)$

In formula, $x\left(t\right)=\left[\begin{array}{c}{x}_{\mathrm{INS}}\left(t\right)\\ x_{\mathrm{VMS}}\left(t\right)\end{array}\right]$ Represent the state vector of integrated navigation system dynamic error model, inertial navigation system move State vector x of state error model_INSState vector x of (t) and tachymeter dynamic error model_VMST () forms； $w\left(t\right)=\left[\begin{array}{c}{w}_{\mathrm{INS}}\left(t\right)\\ w_{\mathrm{VMS}}\left(t\right)\end{array}\right]$ Represent the system noise of integrated navigation system dynamic error model, by inertial navigation system dynamic error mould System noise w of type_INSSystem noise w of (t) and tachymeter dynamic error model_VMST () forms； $F\left(t\right)=\left[\begin{array}{cc}{F}_{\mathrm{INS}}\left(t\right)& 0_{15\×3}\\ o_{3\×15}& F_{\mathrm{VMS}}\left(t\right)\end{array}\right]$ For integrated navigation system dynamic error transfer matrix, O_15×3Represent the null matrix of 15 row 3 row； O_3×15Represent the null matrix of 3 row 15 row；Represent the derivative of integrated navigation system dynamic error model state vector.

Integrated navigation system dynamic error model is carried out sliding-model control, can obtain:

X (k)=F (k, k-1) x (k-1)+w (k) (6)

In formula, F (k, k-1) represent k-1 moment to the k moment integrated navigation system dynamic error transfer matrix, x (k) be from The state vector of the k moment integrated navigation system dynamic error model after dispersion；W (k) represents that the integrated navigation system in k moment is dynamic The system noise of error model, be average be zero, variance is the white noise sequence of Q, and Q-value is according to Q_INSAnd Q_VMSDetermine.

Step 4, the positional increment output of calculating inertial navigation system, the positional increment output of Beidou receiver and the position of tachymeter Put increment output.Detailed process is:

(I) output of inertial navigation system positional increment calculates

According to the speed that inertial navigation system is given, calculate the positional increment in j-1 moment to j momentComputing formula is:

$\mathrm{\Δ}{R}_j^{\mathrm{INS}}={\mathrm{\Σ}}_{r=(j-1)\×N_1+1}^{j\×N_1}{V}_T^n\×\mathrm{\Δ}{T}_{\mathrm{INS}}---\left(7\right)$

Wherein, j represents the moment, and for positive integer, the time interval in j-1 moment to j moment is designated as Δ T_Kal, represent Kalman filtering In the cycle, belong to and be manually set, be typically based on carrier running environment and integrated navigation system required precision is determined；ΔT_INSRepresent The inertial navigation system speed output cycle；R represents that inertial navigation system exports the sequential value of result in the j-1 moment to the j moment； Represent the speed that sequential value is r of inertial navigation system output；N₁=Δ T_Kal/ΔT_INS。

(II) output of Beidou receiver positional increment calculates

According to the speed that Beidou receiver is given, consider the lever arm effect between Beidou receiver and inertial navigation system, meter simultaneously Calculate the positional increment in j-1 moment to j momentComputing formula is:

$\mathrm{\Δ}{R}_j^{\mathrm{BD}}={\mathrm{\Σ}}_{q=(j-1)\×N_2+1}^{j\×N_z}{V}_q^{\mathrm{BD}}\×\mathrm{\Δ}{T}_{\mathrm{BD}}+C_j({\mathrm{\ω}}_j\×L^{\mathrm{BD}})\×\mathrm{\Δ}{T}_{\mathrm{Kal}}---\left(8\right)$

Wherein, Δ T_BDRepresent the Beidou receiver speed output cycle；Q represents that Beidou receiver exports knot in the j-1 moment to the j moment The sequential value of fruit；Represent the speed that sequential value is q of Beidou receiver output；N₂=Δ T_kal/ΔT_BD；C_jRepresent inertia The attitude matrix that navigation system exported in the j moment, ω_jRepresent the angular velocity that inertial navigation system exports, L in the j moment^BDRepresent by north Bucket receiver antenna, to the lever arm vector of inertial navigation system casing center, can measure acquisition in advance.

(III) output of tachymeter positional increment calculates

1. the tachymeter scale factor error δ τ in the j-1 moment that step 7 estimates is utilized_j-1(initial value is set to zero) is to tachymeter Distance increment in the j-1 moment to j moment is modified, i.e.s_jFor revised distance increment, For the distance increment gathered before revising；

Angle α is installed in the course in the j-1 moment 2. updated according to step 8_j-1Angle is installed in (initial value is set to zero) and pitching β_j-1(initial value is set to zero) builds directional cosine vector M_j-1:

$M_{j-1}=\left[\begin{array}{c}-\sin \left({\mathrm{\α}}_{j-1}\right)\cos \left({\mathrm{\β}}_{j-1}\right)\\ \cos \left({\mathrm{\α}}_{j-1}\right)\cos \left({\mathrm{\β}}_{j-1}\right)\\ \sin \left({\mathrm{\β}}_{j-1}\right)\end{array}\right]---\left(9\right)$

M_j-1Effect be the distance increment s that tachymeter is recorded_jProject in inertial navigation system body coordinate system.

3. by revised tachymeter distance increment s_jProject in sky, northeast coordinate system, and consider tachymeter and inertial navigation system Between lever arm effect, calculate tachymeter in the positional increment in j-1 moment to j momentComputing formula is:

$\mathrm{\Δ}{R}_j^{\mathrm{VMS}}=C_j{M}_{j-1}{s}_j+C_j({\mathrm{\ω}}_j\×L^{\mathrm{VMS}})\×\mathrm{\Δ}{T}_{\mathrm{Kal}}---\left(10\right)$

Wherein, L^VMSRepresent by the lever arm vector of tachymeter to inertial navigation system casing center, acquisition can be measured in advance.

Step 5, the inertial navigation system positional increment, Beidou receiver positional increment and the increasing of tachymeter position that calculate with step 4 Based on amount, build dynamic error model observation Z_j, computing formula is:

$Z_j=\left[\begin{array}{c}\mathrm{\Δ}{R}_j^{\mathrm{INS}}-\mathrm{\Δ}{R}_j^{\mathrm{VMS}}\\ \mathrm{\Δ}{R}_j^{\mathrm{INS}}-\mathrm{\Δ}{R}_j^{\mathrm{RD}}\end{array}\right]---\left(11\right)$

Step 6, according to the formula (11) in step 5, the sight of the integrated navigation system dynamic error model described in establishment step three Survey equation, as shown in formula (12):

Y_j=H_jx_j+ρ_j (12)

Wherein, Y_jRepresent the observation sequence in j moment；ρ_jRepresent the observational equation of the integrated navigation system dynamic error model in j moment Noise, be average be zero, variance is the white noise sequence of R, and R value is manually set according to actual application environment, and R is nonnegative matrix； H_jFor the observing matrix in j moment, its concrete form such as formula (13) is shown:

$H_j=\left[\begin{array}{c}{o}_{3\×3}{I}_{3\×3}-\left(C_j{M}_{j-1}{s}_j\right)\×o_{3\×6}-C_j{U}_{j-1}{s}_j-C_j{U}_{j-1}{s}_j-C_j{M}_{j-1}{s}_j\\ o_{3\×3}\mathrm{\Δ}{T}_{\mathrm{Kal}}\×I_{3\×3}{o}_{3\×12}\end{array}\right]---\left(13\right)$

Wherein, O_3×3Represent 3 rank null matrix；O_3×6Represent 3 row 6 row null matrix；O_3×12Represent 3 row 12 row null matrix；I_3×3 Represent 3 rank unit matrix；(C_jM_j-1s_j) × represent by C_jM_j-1s_jThe most negative symmetrical matrix constituted；U_j-1Updated by step 8 Angle α is installed in the course in j-1 moment_j-1Angle β is installed in (initial value is set to zero) and pitching_j-1(initial value is set to zero) Formed:

$U_{j-1}=\left[\begin{array}{cc}-\cos \left({\mathrm{\α}}_{j-1}\right)\cos \left({\mathrm{\β}}_{j-1}\right)& \sin \left({\mathrm{\α}}_{j-1}\right)\sin \left({\mathrm{\β}}_{j-1}\right)\\ -\sin \left({\mathrm{\α}}_{j-1}\right)\cos \left({\mathrm{\β}}_{j-1}\right)& -\cos \left({\mathrm{\α}}_{j-1}\right)\sin \left({\mathrm{\β}}_{j-1}\right)\\ 0& \cos \left({\mathrm{\β}}_{j-1}\right)\end{array}\right]---\left(14\right)$

It should be noted that in the present invention that typically there are a following relation in j moment and k moment:

N×(t_k-t_k-1)=t_j-t_j-1 (15)

Wherein, N is positive integer；t_k-t_k-1Represent the time interval in k-1 moment to k moment；t_j-t_j-1Represent the j-1 moment Time interval to the j moment.Formula (15) shows to carry out integrated navigation system the frequency of the frequency ratio Kalman filtering of discretization Hurry up.

Step 7, error state x (k) of integrated navigation system shown in formula (6) is estimated.

Observational equation in the integrated navigation system dynamic error model set up according to step 3 and step 6, integrating step five is given Observation, use Kalman filter state vector x (k) of integrated navigation system dynamic error model is estimated.Specifically For:

Kalman filter is classic card Thalmann filter, calculates shown in process such as formula (16)～(20):

$\hat{x}(k,k-1)=F(k,k-1)\hat{x}(k-1)---\left(16\right)$

P (k, k-1)=F (k, k-1) P (k-1) [F (k, k-1)]^T+Q (17)

$K\left(k\right)=P(k,k-1)H_k^T{[H_{k}P(k,k-1)H_k^T+R]}^{-1}---\left(18\right)$

$\hat{x}\left(k\right)=\hat{x}(k,k-1)+K\left(k\right)[Z_k-H_k\hat{x}(k,k-1)]---\left(19\right)$

P (k)=P (k, k-1)-K (k) H_kP (k, k-1) (20)

Wherein,Represent the one-step prediction of state vector x (k) of integrated navigation system dynamic error model；Table Show the estimated value of state vector x (k) of integrated navigation system dynamic error model；P (k, k-1) represents one-step prediction variance；P(k) Represent estimate variance；K (k) represents filtering gain.

Knowable to formula (15), the discretization cycle of integrated navigation system dynamic error model and Kalman filtering cycle are the most not Unanimously, when carrying out the calculating of formula (16)～(20), if k ≠ j, the most only carry out state recursion, even filtering gain K (k) Null matrix for equal dimension；If k=j, then carry out above-mentioned complete computation.

The estimated value of k moment integrated navigation system dynamic error model state vector x (k) is i.e. can get through above-mentioned stepsBag Containing site error (δ P)_k, velocity error (δ Vⁿ)_k, misalignmentGyroscope zero offset error (δ ε_g)_k, accelerometer bias by mistake DifferenceAngle error (δ α) is installed in course_k, pitching install angle error (δ β)_kWith tachymeter scale factor error (δ τ)_k。

Step 8, estimated result according to step 7 carry out error correction to integrated navigation system.

The position of k moment inertial navigation system is exported by the error estimation result utilizing step 7 to obtainSpeed exportsAnd appearance State Output matrix C_kIt is corrected, simultaneously ε inclined to gyroscope zero_g, accelerometer biasAngle α, pitching peace are installed in course Clamping angle beta and tachymeter calibration factor τ are updated, and computing formula is:

$P_k^{\mathrm{Cor}}=P_k^{\mathrm{INS}}-{\left(\mathrm{\δP}\right)}_k---\left(21\right)$

$V_k^{\mathrm{Cor}}=V_k^n-{\left(\mathrm{\δ}{V}^n\right)}_k---\left(22\right)$

(ε_g)_k=(ε_g)_k-1-(δε_g)_k (24)

${\left({\▿}_a\right)}_k={\left({\▿}_a\right)}_{k-1}-{\left(\mathrm{\δ}{\▿}_a\right)}_k---\left(25\right)$

α_k=α_k-1-(δα)_k (26)

β_k=β_k-1-(δβ)_k (27)

τ_k=[1-(δ τ)_k]×τ_k-1 (28)

Wherein,WithRepresent position, speed and the attitude matrix after correction respectively；× represent byStructure The most negative symmetrical matrix become；(ε_g)_kWithRepresenting the gyroscope zero in k moment respectively partially and accelerometer bias, initial value is Three-dimensional null vector；α_k、β_kAnd τ_kRepresent that angle, pitching installation angle and tachymeter calibration factor are installed in the course in k moment respectively, Initial value is disposed as zero.

When inertial navigation system carries out subsequent time navigation calculation, utilize (ε_g)_kWithTo the angular velocity gathered and specific force Compensate, and utilize α_k、β_kAnd τ_kVariable corresponding in step 4 and step 6 is updated.

Step 9, integrated navigation system dynamic error model Matrix of shifting of a step F (k, k-1) in step one, two and three is entered Row updates, and the value of k+1 is assigned to k, and arranging system mode is 18 dimension null vectors, then returnes to step 4.

In a particular embodiment of the present invention, inertial navigation system being arranged in load car compartment, Beidou receiver is arranged on roof, Tachymeter is connected with driving wheel bearing by flexible axle.Inertial navigation system complete initially to be directed at after setting in motion, movement locus as figure Shown in 1, its abscissa is latitude (unit: degree), and vertical coordinate is longitude (unit: degree), 2400 seconds sport car time.Inertia The random drift of 3 gyroscopes that navigation system comprises is 0.01 °/h, and constant value drift is 0.02 °/h；Inertial navigation system 3 the accelerometer random drifts comprised are 50 μ g, and constant value drift is 100 μ g.The horizontal positioning accuracy of Beidou receiver It is 3 meters (1 σ).The initial calibration factor of tachymeter is 0.01 meter/pulse.The implementation process of the present invention is as follows:

Step one, the inertial navigation system set up including site error, velocity error, misalignment and inertia device drift error System dynamic error model；Dynamic error model is Φ angle error equation or Ψ angle error equation.In this example, dynamic error model is Φ angle Error equation, its form such as formula (1) is described, wherein, F_INST () is expressed as:

$F_{\mathrm{INS}}\left(t\right)=\left[\begin{array}{ccccc}{F}_{11}& F_{12}& O_{3\×3}& O_{3\×3}& O_{3\×3}\\ F_{21}& F_{22}& F_{23}& O_{3\×3}& F_{25}\\ F_{31}& F_{32}& F_{33}& F_{34}& O_{3\×3}\\ O_{6\×3}& O_{6\×3}& O_{6\×3}& O_{6\×3}& O_{6\×3}\end{array}\right]---\left(29\right)$

The form of each matrix-block is expressed as:

$F_{11}=\left[\begin{array}{ccc}\frac{{\mathrm{\ρ}}_E{R}_{\mathrm{mm}}}{R_m+h}& 0& \frac{{\mathrm{\ρ}}_E}{R_m+h}\\ {\mathrm{\ρ}}_N\sec \left(L\right)(\tan \left(L\right)-\frac{R_{\mathrm{tt}}}{R_t+h})& 0& -\frac{{\mathrm{\ρ}}_N\sec \left(L\right)}{R_t+h}\\ 0& 0& 0\end{array}\right]---\left(30\right)$

In formula, [ρ_E ρ_N ρ_U] represent the terrestrial coordinate system angle of rotation speed relative to geographic coordinate system；L and h represents carrier respectively Residing latitude and elevation；R_mAnd R_tRepresent prime vertical and meridian radius respectively；R_mmRepresent that the radius of prime vertical is about latitude Derivation, R_ttRepresent that meridian radius, about latitude derivation, can be expressed as follows respectively:

$R_{\mathrm{mm}}=\∂R_m/\∂L=6\×R_0\×e\×\sin \left(L\right)\×\cos \left(L\right)---\left(31\right)$

$R_{\mathrm{tt}}={\∂R}_t/\∂L=2\×R_0\×e\×\sin \left(L\right)\×\cos \left(L\right)---\left(32\right)$

Wherein, R₀For earth radius, e is ellipticity.

$F_{12}=\left[\begin{array}{ccc}0& \frac{1}{R_m+h}& 0\\ \frac{\sec \left(L\right)}{R_t+h}& 0& 0\\ 0& 0& 1\end{array}\right]---\left(33\right)$

$F_{21}=\left[\begin{array}{ccc}({2\mathrm{\ω}}_N+{\mathrm{\ρ}}_N{\sec }^2\left(L\right)-\frac{{\mathrm{\ρ}}_U{R}_{\mathrm{tt}}}{R_t+h})v_N+(2{\mathrm{\ω}}_U+\frac{{\mathrm{\ρ}}_N{R}_{\mathrm{tt}}}{R_t+h})v_U& 0& \frac{v_U{\mathrm{\ρ}}_N}{R_t+h}-\frac{{\mathrm{\ρ}}_N\tan \left(L\right)}{R_t+h}{v}_U\\ (-2{\mathrm{\ω}}_N-{\mathrm{\ρ}}_N{\sec }^2\left(L\right)+\frac{{\mathrm{\ρ}}_U{R}_{\mathrm{tt}}}{R_t+h})v_E-\frac{{\mathrm{\ρ}}_E{R}_{\mathrm{mm}}}{R_m+h}{v}_U& 0& \frac{{\mathrm{\ρ}}_N\tan \left(L\right)}{R_t+h}{v}_E-\frac{v_U{\mathrm{\ρ}}_E}{R_m+h}\\ \frac{{\mathrm{\ρ}}_E{R}_{\mathrm{mm}}}{R_m+h}{v}_N-(2{\mathrm{\ω}}_U+\frac{{\mathrm{\ρ}}_N{R}_{\mathrm{tt}}}{R_t+h})v_E& 0& \frac{v_N{\mathrm{\ρ}}_E}{R_m+h}-\frac{v_E{\mathrm{\ρ}}_N}{R_t+h}\end{array}\right]---\left(34\right)$

In formula, [ω_E ω_N ω_U]^TRepresent the component in earth rotation angular speed direction, sky northeastward；[v_E v_N v_U]^TRepresent and carry The movement velocity in body direction, sky northeastward.

$F_{22}=\left[\begin{array}{ccc}\frac{\tan \left(L\right)v_N-v_U}{R_t+h}& 2{\mathrm{\ω}}_U+{\mathrm{\ρ}}_U& -2{\mathrm{\ω}}_N-{\mathrm{\ρ}}_N\\ -2{\mathrm{\ω}}_U-2{\mathrm{\ρ}}_U& -\frac{v_U}{R_m+h}& {\mathrm{\ρ}}_E\\ 2{\mathrm{\ω}}_N+2{\mathrm{\ρ}}_N& -2{\mathrm{\ρ}}_E& 0\end{array}\right]---\left(35\right)$

$F_{23}=\left[\begin{array}{ccc}0& -f_U& f_N\\ f_U& 0& -f_E\\ -f_N& f_E& 0\end{array}\right]---\left(36\right)$

In formula, [f_E f_N f_U]^TRepresent the east orientation of accelerometer measures, north orientation and sky to specific force.

$F_{25}=C_b^n---\left(37\right)$

In formula,Represent attitude matrix.

$F_{31}=\left[\begin{array}{ccc}-\frac{{\mathrm{\ρ}}_E{R}_{\mathrm{mm}}}{R_m+h}& 0& -\frac{{\mathrm{\ρ}}_E}{R_m+h}\\ -{\mathrm{\ω}}_U-\frac{{\mathrm{\ρ}}_N{R}_{\mathrm{tt}}}{R_t+h}& 0& -\frac{{\mathrm{\ρ}}_N}{R_t+h}\\ {\mathrm{\ω}}_N+{\mathrm{\ρ}}_N{\sec }^2\left(L\right)-\frac{{\mathrm{\ρ}}_N\tan \left(L\right)}{R_t+h}& 0& -\frac{{\mathrm{\ρ}}_N\tan \left(L\right)}{R_t+h}\end{array}\right]---\left(38\right)$

$F_{32}=\left[\begin{array}{ccc}0& -\frac{1}{R_m+h}& 0\\ \frac{1}{R_t+h}& 0& 0\\ \frac{\tan \left(L\right)}{R_t+h}& 0& 0\end{array}\right]---\left(39\right)$

$F_{33}=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_U+{\mathrm{\ρ}}_U& -{\mathrm{\ω}}_N-{\mathrm{\ρ}}_N\\ -{\mathrm{\ω}}_U-{\mathrm{\ρ}}_U& 0& {\mathrm{\ρ}}_E\\ {\mathrm{\ω}}_N+{\mathrm{\ρ}}_N& -{\mathrm{\ρ}}_E& 0\end{array}\right]---\left(40\right)$

$F_{34}=-C_b^n---\left(41\right)$

Above-mentioned concrete inertial navigation system dynamic error model is carried out sliding-model control, obtains consistent with formula (2) discrete Change form:

x_INS(k)=F_INS(k, k-1) x_INS(k-1)+w_INS(k) (42)

In this example, discretization period Δ T_Dis=0.1.

Step 2, foundation include installing angle (including that course is installed angle and installed angle with pitching) and tachymeter calibration factor misses Difference is at interior tachymeter dynamic error model.Tachymeter dynamic error model represents such as formula (3), wherein,

F_VMS(t)=O_3×3 (43)

Above-mentioned concrete tachymeter dynamic error model is carried out sliding-model control, obtains the discrete form consistent with formula (4):

x_VMS(k)=F_VMS(k, k-1) x_VMS(k-1)+w_VMS(k) (44)

Wherein, according to discretization formula,

F_VMS(k, k-1)=I_3×3。

Step 3, by state vector x of tachymeter dynamic error model_VMST () expands to inertial navigation system dynamic error model In, constitute the integrated navigation system dynamic error model as shown in formula (5).Understand according to formula (29) and formula (43), F (t) form in formula (5) is:

$F\left(t\right)=\left[\begin{array}{cccccc}{F}_{11}& F_{12}& O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}\\ F_{21}& F_{22}& F_{23}& O_{3\×3}& F_{25}& O_{3\×3}\\ F_{31}& F_{32}& F_{33}& F_{34}& O_{3\×3}& O_{3\×3}\\ O_{9\×3}& O_{9\×3}& O_{9\×3}& O_{9\×3}& O_{9\×3}& O_{9\×3}\end{array}\right]---\left(45\right)$

Above-mentioned concrete integrated navigation system dynamic error model is carried out sliding-model control, obtains consistent with formula (6) discrete Change form:

X (k)=F (k, k-1) x (k-1)+w (k) (46)

In this example, variance matrix Q=diag{ (0.1) of w (k)², (0.1)², (0.2)², (0.001)², (0.001)², (0.001)², (0.01)², (0.01)², (0.01)², 0,0,0,0,0,0,0,0,0}.

Step 4, the positional increment output of calculating inertial navigation system, the positional increment output of Beidou receiver and the position of tachymeter Putting increment output, detailed process is:

(I) output of inertial navigation system positional increment calculates

According to the speed that inertial navigation system is given, calculate the positional increment in j-1 moment to j momentComputing formula is:

$\mathrm{\Δ}{R}_j^{\mathrm{INS}}={\mathrm{\Σ}}_{r=(j-1)\×N_1+1}^{j\×N_1}{V}_r^n\×\mathrm{\Δ}{T}_{\mathrm{INS}}---\left(47\right)$

Wherein, j represents the moment, and for positive integer, the time interval in j-1 moment to j moment is designated as Δ T_Kal, represent Kalman filtering In the cycle, belong to and be manually set, be typically based on carrier running environment and integrated navigation system required precision is determined；ΔT_INSRepresent The inertial navigation system speed output cycle；R represents that inertial navigation system exports the sequential value of result in the j-1 moment to the j moment； Represent the speed that sequential value is r of inertial navigation system output；N₁=Δ T_Kal/ΔT_INS。

In this example, Δ T_Kal=1, Δ T_INS=0.01, N₁=100.

(II) output of Beidou receiver positional increment calculates

According to the speed that Beidou receiver is given, consider the lever arm effect between Beidou receiver and inertial navigation system, meter simultaneously Calculate the positional increment in j-1 moment to j momentComputing formula is:

$\mathrm{\Δ}{R}_j^{\mathrm{BD}}={\mathrm{\Σ}}_{q=(j-1)\×N_2+1}^{j\×N_2}{V}_q^{\mathrm{BD}}\×\mathrm{\Δ}{T}_{\mathrm{BD}}+C_j({\mathrm{\ω}}_j\×L^{\mathrm{BD}})\×\mathrm{\Δ}{T}_{\mathrm{Kal}}---\left(48\right)$

Wherein, Δ T_BDRepresent the Beidou receiver speed output cycle；Q represents that Beidou receiver exports knot in the j-1 moment to the j moment The sequential value of fruit；Represent the speed that sequential value is q of Beidou receiver output；N₂=Δ T_Kal/ΔT_BD；C_jRepresent inertia The attitude matrix that navigation system exported in the j moment, ω_jRepresent the angular velocity that inertial navigation system exports, L in the j moment^BDRepresent by north Bucket receiver antenna, to the lever arm vector of inertial navigation system casing center, can measure acquisition in advance.

In this example, Δ T_BD=0.2, N₂=5, L^BD=[0；-0.1；-1].

(III) output of tachymeter positional increment calculates

1. the tachymeter scale factor error δ τ in the j-1 moment that step 7 estimates is utilized_j-1(initial value is set to zero) is to tachymeter Distance increment in the j-1 moment to j moment is modified, i.e.s_jFor revised distance increment, For the distance increment gathered before revising；

Angle α is installed in the course in the j-1 moment 2. updated according to step 8_j-1Angle is installed in (initial value is set to zero) and pitching β_j-1(initial value is set to zero) builds directional cosine vector M_j-1:

$M_{j-1}=\left[\begin{array}{c}-\sin \left({\mathrm{\α}}_{j-1}\right)\cos \left({\mathrm{\β}}_{j-1}\right)\\ \cos \left({\mathrm{\α}}_{j-1}\right)\cos \left({\mathrm{\β}}_{j-1}\right)\\ \sin \left({\mathrm{\β}}_{j-1}\right)\end{array}\right]---\left(49\right)$

M_j-1Effect be the distance increment s that tachymeter is recorded_jProject in inertial navigation system body coordinate system.

3. by revised tachymeter distance increment s_jProject in sky, northeast coordinate system, and consider tachymeter and inertial navigation system Between lever arm effect, calculate tachymeter in the positional increment in j-1 moment to j momentComputing formula is:

$\mathrm{\Δ}{R}_j^{\mathrm{VMS}}=C_j{M}_{j-1}{s}_j{C}_j({\mathrm{\ω}}_j\×L^{\mathrm{VMS}})\×\mathrm{\Δ}{T}_{\mathrm{Kal}}---\left(50\right)$

Wherein, L^VMSRepresent by the lever arm vector of tachymeter to inertial navigation system casing center, acquisition can be measured in advance.

In this example, L^VMS=[0.1；0；0.3].

Step 5, the inertial navigation system positional increment, Beidou receiver positional increment and the increasing of tachymeter position that calculate with step 4 Based on amount, build dynamic error model observation Z_j:

$Z_j=\left[\begin{array}{c}\mathrm{\Δ}{R}_j^{\mathrm{INS}}-\mathrm{\Δ}{R}_j^{\mathrm{VMS}}\\ \mathrm{\Δ}{R}_j^{\mathrm{INS}}-\mathrm{\Δ}{R}_j^{\mathrm{BD}}\end{array}\right]---\left(51\right)$

Step 6, according to the formula (11) in step 5, the sight of the integrated navigation system dynamic error model described in establishment step three Survey equation:

Y_j=H_jx_j+ρ_j (52)

Wherein, Y_jRepresent the observation sequence in j moment；ρ_jRepresent the observational equation of the integrated navigation system dynamic error model in j moment Noise, be average be zero, variance is the white noise sequence of R, and R value is manually set according to actual application environment, and R is nonnegative matrix, In this example, R=diag{ (0.2)², (0.2)², (0.2)², (0.05)², (0.05)², (0.05)²}；H_jFor the observing matrix in j moment, it is concrete Form is:

$H_j=\left[\begin{array}{c}{O}_{3\×3}{I}_{3\×3}-\left(C_j{M}_{j-1}{s}_j\right)\×O_{3\×6}-C_j{U}_{j-1}{s}_j-C_j{M}_{j-1}{s}_j\\ O_{3\×3}\mathrm{\Δ}{T}_{\mathrm{Kal}}\×l_{3\×3}{O}_{3\×12}\end{array}\right]---\left(53\right)$

Wherein, O_3×3Represent 3 rank null matrix；O_3×6Represent 3 row 6 row null matrix；O_3×12Represent 3 row 12 row null matrix；I_3×3 Represent 3 rank unit matrix；(C_jM_j-1s_j) × represent by C_jM_j-1s_jThe most negative symmetrical matrix constituted；U_j-1Updated by step 8 Angle α is installed in the course in j-1 moment_j-1Angle β is installed in (initial value is set to zero) and pitching_j-1(initial value is set to zero) Formed:

$U_{j-1}=\left[\begin{array}{cc}-\cos \left({\mathrm{\α}}_{j-1}\right)\cos \left({\mathrm{\β}}_{j-1}\right)& \sin \left({\mathrm{\α}}_{j-1}\right)\sin \left({\mathrm{\β}}_{j-1}\right)\\ -\sin \left({\mathrm{\α}}_{j-1}\right)\cos \left({\mathrm{\β}}_{j-1}\right)& -\cos \left({\mathrm{\α}}_{j-1}\right)\sin \left({\mathrm{\β}}_{j-1}\right)\\ 0& \cos \left({\mathrm{\β}}_{j-1}\right)\end{array}\right]---\left(54\right)$

It should be noted that in the present invention j moment and k moment-as have a following relation:

N×(t_k-t_k-1)=t_j-t_j-1 (55)

Wherein, N is positive integer, and this example takes N=10.

Step 7, error state x (k) to integrated navigation system are estimated.

Observational equation in the integrated navigation system dynamic error model set up according to step 3 and step 6, integrating step five is given Observation, use Kalman filter state vector x (k) of integrated navigation system dynamic error model is estimated.Specifically For:

Kalman filter is classic card Thalmann filter, calculates shown in process such as formula (56)～(60):

$\hat{x}(k,k-1)=F(k,k-1)\hat{x}(k-1)---\left(56\right)$

P (k, k-1)=F (k, k-1) P (k-1) [F (k, k-1)]^T+Q (57)

$K\left(k\right)=P(k,k-1)H_k^T{[H_{k}P(k,k-1)H_k^T+R]}^{-1}---\left(58\right)$

$\hat{x}\left(k\right)=\hat{x}(k,k-1)+K\left(k\right)[Z_k-H_k\hat{x}(k,k-1)]---\left(59\right)$

P (k)=P (k, k-1)-K (k) H_kP (k, k-1) (60)

Wherein,Represent the one-step prediction of state vector x (k) of integrated navigation system dynamic error model；Table Show the estimated value of state vector x (k) of integrated navigation system dynamic error model；P (k, k-1) represents one-step prediction variance；P(k) Represent estimate variance；K (k) represents filtering gain.

In this example,Value is 18 dimension null vectors, P (0)=diag{ (1)², (1)², (2)², (0.01)², (0.01)², (0.01)², (0.00052)², (0.00052)², (0.00087)², (0.001)², (0.001)², (0.001)², (0.01)², (0.01)², (0.01)², (0.0017)², (0.0017)², (0.001)²}。

When carrying out the calculating of formula (56)～(60), if k ≠ j, the most only carry out state recursion, even filtering gain K (k) It it is the null matrix of 18 × 18；If k=j, then carry out above-mentioned complete computation.

The estimated value of k moment integrated navigation system dynamic error model state vector x (k) is i.e. can get through above-mentioned stepsBag Containing site error (δ P)_k, velocity error (δ Vⁿ)_k, misalignmentGyroscope zero offset error (δ ε_g)_k, accelerometer bias by mistake DifferenceAngle error (δ α) is installed in course_k, pitching install angle error (δ β)_kWith tachymeter scale factor error (δ τ)_k。

Step 8, estimated result according to step 7 carry out error correction to integrated navigation system.

The position of k moment inertial navigation system is exported by the error estimation result utilizing step 7 to obtainSpeed exportsAnd appearance State Output matrix C_kIt is corrected, simultaneously ε inclined to gyroscope zero_g, accelerometer biasAngle α, pitching peace are installed in course Clamping angle beta and tachymeter calibration factor τ are updated, and computing formula is:

$P_k^{\mathrm{Cor}}=P_k^{\mathrm{INS}}-{\left(\mathrm{\δP}\right)}_k---\left(61\right)$

$V_k^{\mathrm{Cor}}=V_k^n-{\left(\mathrm{\δ}{V}^n\right)}_k---\left(62\right)$

(ε_g)_k=(ε_g)_k-1-(δε_g)_k (64)

${\left({\▿}_a\right)}_k={\left({\▿}_a\right)}_{k-1}-{\left(\mathrm{\δ}{\▿}_a\right)}_k---\left(65\right)$

α_k=α_k-1-(δα)_k (66)

β_k=β_k-1-(δβ)_k (67)

τ_k=[1-(δ τ)_k]×τ_k-1 (68)

Wherein,WithRepresent position, speed and the attitude matrix after correction respectively；× represent byStructure The most negative symmetrical matrix become；(ε_g)_kWithRepresenting the gyroscope zero in k moment respectively partially and accelerometer bias, initial value is Three-dimensional null vector；α_k、β_kAnd τ_kRepresent that angle, pitching installation angle and tachymeter calibration factor are installed in the course in k moment respectively, Initial value is disposed as zero.

When inertial navigation system carries out subsequent time navigation calculation, utilize (ε_g)_kWithTo the angular velocity gathered and specific force Compensate, and utilize α_k、β_kAnd τ_kVariable corresponding in step 4 and step 6 is updated.

Step 9, integrated navigation system dynamic error model Matrix of shifting of a step F (k, k-1) in step one, two and three is entered Row updates, and the value of k+1 is assigned to k, and arranging system mode is 18 dimension null vectors, then returnes to step 4.

Using the present invention to estimate the installation angle of inertial navigation system and tachymeter, angle and pitching are installed in the course of gained Installing angle respectively the most as shown in Figures 2 and 3, its abscissa is time (unit: second), vertical coordinate for install angle (unit: Degree).Can be seen that from this two width figure, course installation angle and pitching installation angle Fast Convergent after estimating to start, and respectively at Tend to constant value after 170 seconds and 90 seconds, illustrate that installation angle can be measured by method that the present invention proposes rapidly and accurately.

The above is only the preferred embodiment of the present invention, it is noted that for those skilled in the art, Under the premise without departing from the principles of the invention, it is also possible to make some improvement, or wherein portion of techniques feature is carried out equivalent to replace Changing, these improve and replace and also should be regarded as protection scope of the present invention.

Claims (1)

1. one kind utilizes the Big Dipper to measure the method that inertial navigation system installs angle with tachymeter, it is characterised in that by inertial navigation System is arranged in load car compartment, and Beidou receiver is arranged on roof, and tachymeter is connected with driving wheel bearing by flexible axle, inertia Navigation system complete initially to be directed at after setting in motion, its abscissa is latitude, and vertical coordinate is longitude；Inertial navigation system comprises The random drift of 3 gyroscopes is 0.01 °/h, and constant value drift is 0.02 °/h；3 acceleration that inertial navigation system comprises Meter random drift is 50 μ g, and constant value drift is 100 μ g, and the initial calibration factor of tachymeter is 0.01 meter/pulse, including step:

Step one, setting up inertial navigation system dynamic error model, state variable includes site error, velocity error, misalignment With inertia device drift error:

Northeastward under sky coordinate system, inertial navigation system dynamic error model is:

In formula, t is time value, is arithmetic number；x_INS(t) table Show the state vector of inertial navigation system dynamic error model, by site error δ P, velocity error δ Vⁿ, misalignmentGyro Instrument zero offset error δ ε_gWith accelerometer bias errorComposition；w_INST () represents the system of inertial navigation system dynamic error model Noise；F_INST () is inertial navigation system dynamic error transfer matrix；Represent inertial navigation system dynamic error model shape The derivative of state vector；

Inertial navigation system dynamic error model is carried out sliding-model control, obtains:

x_INS(k)=F_INS(k, k-1) x_INS(k-1)+w_INS(k)

In formula, k represents the moment, and for positive integer, the time interval in k-1 moment to k moment is designated as Δ T_Dis, represent error model The discretization cycle；F_INS(k, k-1) represents the inertial navigation system dynamic error transfer matrix in k-1 moment to k moment； x_INSK () is the state vector of the k moment inertial navigation system after discretization；w_INSK () represents that the inertial navigation system in k moment is moved The system noise of state error model, be average be zero, variance is Q_INSWhite noise sequence, Q_INSFor nonnegative matrix；

Step 2, setting up tachymeter dynamic error model, the state variable that this model comprises is for installing angle error and and tachymeter Scale factor error, wherein installs angle error and includes that course is installed angle error and installed angle error with pitching,

Tachymeter dynamic error model is: ${\stackrel{\·}{x}}_{\mathrm{VMS}}\left(t\right)=F_{\mathrm{VMS}}\left(t\right)x_{\mathrm{VMS}}\left(t\right)+w_{\mathrm{VMS}}\left(t\right)$

In formula, x_VMS(t)=[δ α δ β δ τ]^T, x_VMST () represents the state vector of tachymeter dynamic error model, by course Install angle error δ α, angle error δ β and tachymeter scale factor error δ τ composition is installed in pitching；w_VMST () represents tachymeter The system noise of dynamic error model；F_VMST () is tachymeter dynamic error transfer matrix；Represent that tachymeter dynamically misses The derivative of differential mode type state vector；

Tachymeter dynamic error model is carried out sliding-model control, obtains:

x_VMS(k)=F_VMS(k, k-1) x_VMS(k-1)+w_VMS(k)

In formula, F_VMS(k, k-1) represents the tachymeter dynamic error transfer matrix in k-1 moment to k moment；x_VMS(k) be from The state vector of the k moment tachymeter dynamic error model after dispersion；w_VMSK () represents the tachymeter dynamic error model in k moment System noise, be average be zero, variance is Q_VMSWhite noise sequence, Q_VMSFor nonnegative matrix；

The inertial navigation system dynamic error model that step 3, combining step one are set up, and the tachymeter that step 2 is set up is dynamic Error model, structure integrated navigation system dynamic error model:

In formula, $x\left(t\right)=\left[\begin{array}{c}{x}_{\mathrm{INS}}\left(t\right)\\ x_{\mathrm{VMS}}\left(t\right)\end{array}\right]$ Represent the state vector of integrated navigation system dynamic error model, inertial navigation system move State vector x of state error model_INSState vector x of (t) and tachymeter dynamic error model_VMST () forms； $w\left(t\right)=\left[\begin{array}{c}{w}_{\mathrm{INS}}\left(t\right)\\ w_{\mathrm{VMS}}\left(t\right)\end{array}\right]$ Represent the system noise of integrated navigation system dynamic error model, by inertial navigation system dynamic error mould System noise w of type_INSSystem noise w of (t) and tachymeter dynamic error model_VMST () forms； $F\left(t\right)=\left[\begin{array}{cc}{F}_{\mathrm{INS}}\left(t\right)& 0_{15\×3}\\ 0_{3\×15}& F_{\mathrm{VMS}}\left(t\right)\end{array}\right]$ For integrated navigation system dynamic error transfer matrix, 0_15×3Represent the null matrix of 15 row 3 row； 0_3×15Represent the null matrix of 3 row 15 row；Represent the derivative of integrated navigation system dynamic error model state vector；

Integrated navigation system dynamic error model is carried out sliding-model control, obtains:

X (k)=F (k, k-1) x (k-1)+w (k)

In formula, F (k, k-1) represent k-1 moment to the k moment integrated navigation system dynamic error transfer matrix, x (k) be from The state vector of the k moment integrated navigation system dynamic error model after dispersion；W (k) represents that the integrated navigation system in k moment is dynamic The system noise of error model, be average be zero, variance is the white noise sequence of Q, and Q-value is according to Q_INSAnd Q_VMSDetermine；

Step 4, the positional increment output of calculating inertial navigation system, the positional increment output of Beidou receiver and the position of tachymeter Putting increment output, detailed process is:

(I) output of inertial navigation system positional increment is calculated

According to the speed that inertial navigation system is given, calculate the positional increment in j-1 moment to j momentComputing formula is:

${\mathrm{\ΔR}}_j^{\mathrm{INS}}={\mathrm{\Σ}}_{r=(j-1)\×N_1+1}^{j\×N_1}{V}_{\mathrm{\τ}}^n\×{\mathrm{\ΔT}}_{\mathrm{INS}}$

Wherein, j represents the moment, and for positive integer, the time interval in j-1 moment to j moment is designated as Δ T_Kal, represent Kalman filtering In the cycle, determine according to carrier running environment and integrated navigation system required precision；ΔT_INSRepresent inertial navigation system speed output week Phase；R represents that inertial navigation system exports the sequential value of result in the j-1 moment to the j moment；Represent inertial navigation system output Sequential value is the speed of r；N₁=Δ T_kal/ΔT_INS；

(II) output of Beidou receiver positional increment is calculated

According to the speed that Beidou receiver is given, consider the lever arm effect between Beidou receiver and inertial navigation system, meter simultaneously Calculate the positional increment in j-1 moment to j momentComputing formula is:

${\mathrm{\ΔR}}_j^{\mathrm{BD}}={\mathrm{\Σ}}_{q=(j-1)\×N_2+1}^{j\×N_2}{V}_q^{\mathrm{BD}}\×{\mathrm{\ΔT}}_{\mathrm{BD}}+C_j({\mathrm{\ω}}_j\×L^{\mathrm{BD}})\×{\mathrm{\ΔT}}_{\mathrm{Kal}}$

Wherein, Δ T_BDRepresent the Beidou receiver speed output cycle；Q represents that Beidou receiver exports knot in the j-1 moment to the j moment The sequential value of fruit；Represent the speed that sequential value is q of Beidou receiver output；N₂=Δ T_Kal/ΔT_BD；C_jRepresent inertia The attitude matrix that navigation system exported in the j moment, ω_jRepresent the angular velocity that inertial navigation system exports, L in the j moment^BDRepresent by north Bucket receiver antenna is vectorial to the lever arm of inertial navigation system casing center；

(III) output of tachymeter positional increment is calculated

1. the tachymeter scale factor error δ τ in the j-1 moment that step 7 estimates is utilized_j-1To tachymeter in the j-1 moment to the j moment Distance increment be modified, i.e.δτ_j-1Initial value is set to zero, s_jIncrease for revised distance Amount,For revising the front distance increment gathered,

Angle α is installed in the course in the j-1 moment 2. updated according to step 8_j-1With pitching, angle β is installed_j-1, build direction cosines Vector M_j-1, M_j-1It is the distance increment s that tachymeter is recorded_jProject in inertial navigation system body coordinate system, α_j-1And β_j-1's Initial value is set to zero,

$M_{j-1}=\left[\begin{array}{c}-\sin \left({\mathrm{\α}}_{j-1}\right)\cos \left({\mathrm{\β}}_{j-1}\right)\\ \cos \left({\mathrm{\α}}_{j-1}\right)\cos \left({\mathrm{\β}}_{j-1}\right)\\ \sin \left({\mathrm{\β}}_{j-1}\right)\end{array}\right]$

3. by revised tachymeter distance increment s_jProject in sky, northeast coordinate system, and consider tachymeter and inertial navigation system Between lever arm effect, calculate tachymeter in the positional increment in j-1 moment to j momentComputing formula is:

${\mathrm{\ΔR}}_j^{\mathrm{VMS}}=C_j{M}_{j-1}{s}_j+C_j({\mathrm{\ω}}_j\×L^{\mathrm{VMS}})\×{\mathrm{\ΔT}}_{\mathrm{Kal}}$

Wherein, L^VMSRepresent by the lever arm vector of tachymeter to inertial navigation system casing center, acquisition can be measured in advance；

Step 5, the inertial navigation system positional increment, Beidou receiver positional increment and the increasing of tachymeter position that calculate with step 4 Based on amount, build dynamic error model observation Z_j, computing formula is:

$Z_j=\left[\begin{array}{c}{\mathrm{\ΔR}}_j^{\mathrm{INS}}-{\mathrm{\ΔR}}_j^{\mathrm{VMS}}\\ {\mathrm{\ΔR}}_j^{\mathrm{INS}}-{\mathrm{\ΔR}}_j^{\mathrm{BD}}\end{array}\right]$

Step 6, according to the formula Z in step 5_j, the observation of the integrated navigation system dynamic error model described in establishment step three Equation:

Y_j=H_jx_j+ρ_j

Wherein, Y_jRepresent the observation sequence in j moment；ρ_jRepresent the observational equation of the integrated navigation system dynamic error model in j moment Noise, be average be zero, variance is the white noise sequence of R, and R value is manually set according to actual application environment, and R is nonnegative matrix； H_jFor the observing matrix in j moment, formula is as follows:

$H_j=\left[\begin{array}{c}\begin{array}{cc}{0}_{3\×3}& I_{3\×3}-\left(C_j{M}_{j-1}{s}_j\right)\×0_{3\×6}-C_j{U}_{j-1}{s}_j-C_j{M}_{j-1}{s}_j\end{array}\\ \begin{array}{ccc}{0}_{3\×3}& {\mathrm{\ΔT}}_{\mathrm{Kal}}\×I_{3\×3}& 0_{3\×12}\end{array}\end{array}\right]$

Wherein, 0_3×3Represent 3 rank null matrix；0_3×6Represent 3 row 6 row null matrix；0_3×12Represent 3 row 12 row null matrix；I_3×3 Represent 3 rank unit matrix；(C_jM_j-1s_j) × represent by C_jM_j-1s_jThe most negative symmetrical matrix constituted；U_j-1Updated by step 8 Angle α is installed in the course in j-1 moment_j-1With pitching, angle β is installed_j-1Formed, α_j-1And β_j-1Initial value be set to zero:

$U_{j-1}=\left[\begin{array}{cc}-\cos \left({\mathrm{\α}}_{j-1}\right)\cos \left({\mathrm{\β}}_{j-1}\right)& \sin \left({\mathrm{\α}}_{j-1}\right)\sin \left({\mathrm{\β}}_{j-1}\right)\\ -\sin \left({\mathrm{\α}}_{j-1}\right)\cos \left({\mathrm{\β}}_{j-1}\right)& -\cos \left({\mathrm{\α}}_{j-1}\right)\sin \left({\mathrm{\β}}_{j-1}\right)\\ 0& \cos \left({\mathrm{\β}}_{j-1}\right)\end{array}\right]$

Wherein, there are a following relation in j moment and k moment:

N×(t_k-t_k-1)=t_j-t_j-1

Wherein, N is positive integer；t_k-t_k-1Represent the time interval in k-1 moment to k moment；t_j-t_j-1Represent the j-1 moment Time interval to the j moment；

Observational equation in step 7, the integrated navigation system dynamic error model set up according to step 3 and step 6, in conjunction with step Rapid five observations be given, use Kalman filter to estimate state vector x (k) of integrated navigation system dynamic error model Meter:

Kalman filter is classic card Thalmann filter, calculates process as follows:

$\hat{x}(k,k-1)=F(k,k-1)\hat{x}(k-1)$

P (k, k-1)=F (k, k-1) P (k-1) [F (k, k-1)]^T+Q

$K\left(k\right)=P(k,k-1)H_k^T{[H_{k}P(k,k-1)H_k^T+R]}^{-1}$

$\hat{x}\left(k\right)=\hat{x}(k,k-1)+K\left(k\right)[Z_k-H_k\hat{x}(k,k-1)]$

P (k)=P (k, k-1)-K (k) H_kP (k, k-1)

Wherein,Represent the one-step prediction of state vector x (k) of integrated navigation system dynamic error model；Table Show the estimated value of state vector x (k) of integrated navigation system dynamic error model；P (k, k-1) represents one-step prediction variance；P(k) Represent estimate variance；K (k) represents filtering gain；

If k ≠ j, the most only carry out state recursion, even the null matrix that filtering gain K (k) is equal dimension；If k=j, then enter The above-mentioned complete computation of row；

The estimated value of k moment integrated navigation system dynamic error model state vector x (k) is i.e. can get through above-mentioned stepsBag Containing site error (δ P)_k, velocity error (δ Vⁿ)_k, misalignmentGyroscope zero offset error (δ ε_g)_k, accelerometer bias by mistake DifferenceAngle error (δ α) is installed in course_k, pitching install angle error (δ β)_kWith tachymeter scale factor error (δ τ)_k；

Step 8, estimated result according to step 7 carry out error correction to integrated navigation system, specifically refer to utilize step 7 to obtain The position of k moment inertial navigation system is exported by the error estimation result takenSpeed exportsC is exported with attitude matrix_kEnter Row correction, ε inclined to gyroscope zero simultaneously_g, accelerometer biasAngle α is installed in course, angle β and tachymeter are installed in pitching Calibration factor τ is updated, and computing formula is:

$P_k^{\mathrm{Cor}}=P_k^{\mathrm{INS}}-{\left(\mathrm{\δP}\right)}_k$

$V_k^{\mathrm{Cor}}=V_k^n-{\left({\mathrm{\δV}}^n\right)}_k$

(ε_g)_k=(ε_g)_k-1-(δε_g)_k

${\left({\▿}_{\mathrm{\α}}\right)}_k={\left({\▿}_{\mathrm{\α}}\right)}_{k-1}-{\left({\mathrm{\δ}\▿}_{\mathrm{\α}}\right)}_k$

α_k=α_k-1-(δα)_k

β_k=β_k-1-(δβ)_k

τ_k=[1-(δ τ)_k]×τ_k-1

Wherein,WithRepresent position, speed and the attitude matrix after correction respectively；× represent byStructure The most negative symmetrical matrix become；(ε_g)_kWithRepresenting the gyroscope zero in k moment respectively partially and accelerometer bias, initial value is Three-dimensional null vector；α_k、β_kAnd τ_kRepresent that angle, pitching installation angle and tachymeter calibration factor are installed in the course in k moment respectively, Initial value is disposed as zero；

When inertial navigation system carries out subsequent time navigation calculation, utilize (ε_g)_kWithTo the angular velocity gathered and specific force Compensate, and utilize α_k、β_kAnd τ_kVariable corresponding in step 4 and step 6 is updated；

Step 9, integrated navigation system dynamic error model Matrix of shifting of a step F (k, k-1) in step one, two and three is entered Row updates, and the value of k+1 is assigned to k, and arranging system mode is 18 dimension null vectors, then returnes to step 4.