CN106940193A - A kind of ship self adaptation based on Kalman filter waves scaling method - Google Patents

Abstract

A kind of systematic calibration method based on Kalman filter is devised, the oscillating motion for making full use of ship to be influenceed across the sea by wave and producing produces excitation to IMU, on-line proving in real time is carried out to the systematic parameter of inertia device based on Kalman filter.First, inertia device input/output model is set up.Then, it is determined that the state equation and observational equation of Kalman filter, the excitation for making full use of it to produce inertia device.Finally, systematic error parameter is estimated and compensated again according to the Kalman filter equation of foundation, completes demarcation, update systematic error parameter.Once demarcation may not reach required precision, at this moment, can carry out the demarcation iteration of systematic parameter repeatedly, wave filter can also once be updated to data at regular intervals, then carry out calibration compensation to improve navigation accuracy.It can thus be realized not by the dismounting to inertia device and FOG IMU are re-scaled.

Description

A kind of ship self adaptation based on Kalman filter waves scaling method

(1), technical field

It is particularly long in system the present invention relates to a kind of systematic calibration method based on fiber-optic gyroscope strapdown inertial navigation system During boat, the oscillating motion for being influenceed and being produced by wave across the sea using ship produces excitation to IMU, based on Kalman filter pair The systematic parameter of inertia device carries out on-line proving in real time.

(2), background technology

External naval vessel gyroscope inertial navigation system application technology is more ripe now, and SINS has been able to full The requirement of sufficient navigation level, just towards high accuracy, inexpensive field development.But, due to manufacturing technology level technology limitation with And the block of advanced foreign technology, the precision level that China optical fibre gyro can reach compared with developed country also exist it is certain poor Away from existing inertia device can't meet required required precision.So, only refer to by Curve guide impeller and raising technique Mark is more and more difficult to improve the precision of optical fibre gyro, and its structure becomes increasingly complex, corresponding technical costs more and more higher, development Cycle is elongated, and returns production, I＆ M and make troubles.Therefore systematic error parameter is carried out accurately demarcating and mending The precision repaid to improve inertial navigation system is more meaningful.

Result after Laboratory Calibration is due to the change of the external conditions such as working time, temperature, in practical service environment It may change.For ship gyroscope inertial navigation system, when ship is navigated by water across the sea, inertial navigation system is with ship in sea Oscillating motion is carried out in the presence of wave, under the dynamic environment, each harmful interference can be produced to inertia device internal system parameters Influence, brings it about change.

Because ship is across the sea by the various marine interference such as the concussion of wave, particularly when system length is navigated, FOG-IMU Use time is long, the factor such as the component such as optical fibre gyro internal optical fiber ring deformation so that the systematic error parameter of optical fibre gyro It may change.Further, since change (such as system cut-off, mechanical breakdown, the software fortune that may be produced inside inertial navigation system Row mistake, navigation error accumulation etc.) precision of marine navigator is influenceed, what Laboratory Calibration went out so before dispatching from the factory before is System parameter will be inaccurate, it is necessary to demarcated again to systematic error parameter, and return factory and re-start and demarcate and can additionally increase Addition sheet, the time cycle can also extend, and FOG-IMU is removed and installed and very cumbersome.For these reasons, to ship The Auto-calibration of navigation system is just particularly important.

The present invention devises a kind of systematic calibration method based on Kalman filter, makes full use of ship to ride the sea Shi Haifeng waves act on the oscillating motion produced to ship, and accurate error calibration parameter is motivated again.Inertial navigation is set Standby output parameter is gathered in real time, and error parameter is sufficiently demarcated according to the size of wave, it is possible in real time Obtain new calibrating parameters.In order to improve the accuracy of parameter, once demarcation may not reach required precision, at this moment, can carry out The demarcation iteration of systematic parameter repeatedly, wave filter can also once be updated to data at regular intervals, then be demarcated Compensate to improve navigation accuracy.It can thus be realized not by the dismounting to inertia device to FOG-IMU again Demarcation.

(3), the content of the invention

The purpose of the present invention is to be based on high-precision optical fiber gyro inertial navigation system there is provided one kind when ship length is navigated, profit The online adaptive scaling method of excitation is waved with wave with ship and systematic error parameter is produced, so as to improve navigation essence Degree.

The object of the present invention is achieved like this：

The present invention comprises the following steps：

(1) initial alignment of systematic parameter is completed in laboratory, the related debugging of fiber-optic gyroscope strapdown inertial navigation system is completed Work.

(2) fiber-optic gyroscope strapdown inertial navigation system is arranged on ship center, after preheating, starts to gather the navigation of IMU outputs Data.

(3) determine initial state information to complete system coarse alignment according to IMU output, then determined according to coarse alignment Attitude information pass through a series of strapdowns resolve completion system fine alignment.

(3) inertia device input/output model is set up, it is then determined that the state equation and observational equation of Kalman filter.

(4) during ship's navigation, ship does approximate oscillating motion with wave, makes full use of it to produce inertia device Excitation.

(5) systematic error parameter is estimated and compensated again according to the Kalman filter equation of foundation, completes demarcation, Update systematic error parameter.

Present invention additionally comprises following feature：

1st, the measurement model of inertia device

Due to the limitation of mechanical processing technique, mounting condition equal error factor, a systems of accelerometer system and top in real system G systems of spiral shell system are not orthogonal coordinate system.According to accelerometer and the input/output relation of gyro, accelerometer and top are provided respectively Linear calibration's model of spiral shell passage.

The input and output Mathematical Modeling of accelerometer passage is expressed as：

A^a=δ K^af^b+δB^a+w^a

Wherein,Represent output of the accelerometer under a systems；Represent Force vector is compared in input of the accelerometer under carrier coordinate system b systems；Accelerometer biasSystem Random noiseK^aFor：

Wherein, constant multiplier of the accelerometer on i directions δ K_i(i=x, y, z) is represented, the accelerometer i under a systems The installation coefficient δ M between the measurement axle on the lower accelerometer j directions of measurement axle and b systems on direction_ijRepresent.

Laboratory Calibration is joined typically from high-accuracy turntable as benchmark, but in Kalman filter demarcation estimating system error Do not have turntable benchmark in several processes, so new constraint must be made, a kind of conventional method is defined in the orthogonal seats of IMU Mark system, i.e., under carrier coordinate system, X_bAxle and X_aDirection of principal axis is consistent, Y_bAxle is in O-X_aY_aIn plane, Z_bAxle is true according to the right-hand rule It is fixed.So δ K^aIt will be expressed as again：

Similarly, the Mathematical Modeling of gyro passage is represented by：

T^g=δ S^gω^b+δD^g+w^g

In formula,Represent output of three gyros under g systems；Represent Input angle speed of three gyros under b systems；The zero of three gyros is inclinedThe random gyro of system DriftS^gFor：

In above formula, constant multiplier of the gyro on i directions δ S_i(i=x, y, z) is represented, under g systems on gyro i directions Measurement axle and b systems time gyro j directions on measurement axle between installation coefficient δ E_ijRepresent.

2nd, the state equation of Kalman filter is set up

Choose zero inclined, constant multiplier of velocity error, angular error, accelerometer and the gyro of inertial navigation system and system is installed Number is used as the state variable filtered.It is made up of altogether 27 state parameters：

Wherein,

Wherein, δ V_E、δV_NWith δ V_USystem east orientation, north orientation and sky orientation speed error, δ φ are represented respectively_E、δφ_NWith δ φ_UPoint Not Biao Shi system east orientation, north orientation and day to attitude error.

Using " east, north, day " geographic coordinate system as navigational coordinate system, according to SINS velocity errors and attitude error equations, Set up navigation error equation.Then it is described using linear first-order differential equation, draws the state equation of Kalman filter.

X is 27 dimension system state variableses, and F is 27 × 27 state matrixs for maintaining system, and G is the system noise square of 27 × 6 dimensions Battle array, W maintains system noise vector for 6, and 6 dimension noise vectors can be expressed as with white noise：

The noise transfer matrix of system can be usedTo represent.Wherein,With(i, j= 1,2,3) the strapdown transition matrix of accelerometer and gyro is represented respectively.

The writeable component block matrix form of state-transition matrix, is represented with F：Wherein：

R_eThe radius of the earth is represented, L represents the latitude in SINS locations, ω_ieRepresent earth rotation angular speed, V_EAnd V_NPoint East orientation speed and north orientation speed that Biao Shi be in strapdown solution process.f_E、f_NAnd f_URepresent respectively east orientation that accelerometer measures, North orientation and day are to output specific force.With(i, j=1,2,3) represents the strapdown transition matrix of accelerometer and gyro respectively.ω^b For real gyro angular speed input value.

3rd, the observational equation for the Kalman filter set up：

During parameters revision, it is impossible to the accurate attitude error for obtaining carrier.And inertial navigation system is with Ship Swaying Line is not present in calibration process to move, angular movement is only existed, now the speed of navigation calculation is zero, and position is constant.Pass through speed Integration obtains the positional information of carrier, therefore using velocity error as observed quantity.

Z=[δ V_E δV_N δV_U]^T

Using IMU velocity error as observed quantity, then the measurement equation in filtering equations is represented by：

Z=HX+v

In above formula, H and v are respectively：

H=[diag { 111 } 0_3×24], v=[v_x v_y v_z]^T

Wherein, H represents observing matrix, and v, which is represented, measures noise.

4th, error parameter is compensated

Result is modified using the method based on Kalman filter estimating system error parameter, FOG- can be obtained IMU systematic parameter High Definition Systems error parameter results.Afterwards, systematic parameter is compensated, is added to actual inertial navigation In system, it is determined that whether the parameter of demarcation can improve the precision of system.

The method compensated according to systematic error parameter, inertial navigation system velocity error is arrived by the compensation of smart Calibration Simulation estimated result In attitude error equations, velocity error and attitude error after being compensated, the error comparing result before and after output compensation.

The inventive method has the following advantages that：

1st, ship oscillating motion caused by wave is rationally utilized, realizes that the real-time adaptive of ship waves demarcation, Ship is overcome when system length is navigated, the problem of inertia device demarcation is difficult.

2nd, on the premise of inertia device precision is not improved, it becomes possible to improve the stated accuracy of system, particularly in system During long boat, in the case that systematic parameter easily changes, higher stated accuracy can be also realized.

3rd, compared with common peg model, simple to operate, low cost of the invention, fast convergence rate.

4th, the time interval that can be artificially set by system, can constantly update systematic parameter, and systematic parameter is mended Repay.

5th, inertial navigation itself achieves that the self-recision to systematic parameter, it is not necessary to carry out extra manual hand manipulation or Turntable rotates excitation.

6th, The present invention gives the specific steps that calibration algorithm is carried out inside inertial navigation system, enter according to inventive step and to it Row simulating, verifying.There is important theory significance and construction value for the Auto-calibration of marine navigator parameter.

The explanation beneficial to the present invention：

MATLAB emulation experiments：

1st, simulated conditions are set

Under simulated conditions, it is assumed that fiber-optic gyroscope strapdown inertial navigation system be located at ship center position, inertia device relative to Ship is static in itself, and IMU only does angular movement, without line motion, and has been completed initial alignment.

(1) optical fibre gyro inertial navigation system is under quiescent conditions：

System initial position：45.779 ° of north latitude, 126.671 ° of east longitude；

Terrestrial equator radius is set to：Re=6378137.0m；

Earth rotation angular speed：ω_ie=7.2921158 × 10^-5m/s；

The acceleration of gravity of earth surface：G0=9.78049m/s²；

The turntable sampling time：T=0.01s；

(2) optical fibre gyro inertial navigation system with ship in the presence of wave when being in swinging condition, and parameter state is set It is set to：

Rolling period：T=[8s 5s 5s]^T；

Wave amplitude：ψ_max=[14 ° 10 ° 10 °]^T；

Initial phase：θ₀=[0 ° 0 ° 0 °]^T；

(3) FOG-IMU systematic errors parameter setting：

Initial misalignment：φ₀=[0 ° 0 ° 0 °]^T；

Accelerometer bias：δD^a=[the μ g of 50 μ g, 50 μ g 50]^T；

Accelerometer constant multiplier：δD^a=[50ppm 50ppm 50ppm]^T；

Accelerometer installs coefficient：M_yx=M_zx=M_zy=50 "；

Gyro zero is inclined：D^g=[0.1 °/h, 0.1 °/h, 0.1 °/h]^T；

Gyro installation coefficient：D^g=[50ppm 50ppm 50ppm]^T；

Gyro scale factor error：E_xy=E_xz=E_yx=E_yz=E_zx=E_zy=50 "；

(4) in addition, state variable X initial value X₀, initial covariance matrix initial value P₀, system noise variance matrix Q, Measuring noise square difference battle array R initial set value is as follows：

X₀=[0]_1×27；

R=diag ([10^-6 10^-6 10^-6]g)²；

2nd, emulation experiment process

MATLAB simulation analysis are carried out according to IMU systematic parameters error model, detailed process is as follows：

(1) setting initial parameter and the reference value of inertial device error, define initial position, speed and attitude, give just The parameters such as beginning covariance matrix, system noise variance matrix and observation noise variance matrix；

(2) wave algorithm simulation ship according to setting before and do oscillating motion across the sea, simulation total nominal time sets For 12 minutes, and export real speed and position and then result is preserved；

(3) according to the true of outputCalculate the pose transformation matrix that navigation coordinate is tied to carrier coordinate systemDue to Any error is not brought in the solution procedure of the matrix into, so the matrix is theoretical value；

(4) by the Initial value computing system parameter error amount set before, it is added in real IMU output valves, profit The gyro and accelerometer output data information produced in calibration process is simulated with MATLAB programs, is exported using gyroMeter Calculate analog simulation output

(5) actual emulation output is released with error with Quaternion Method

(6) utilize what is obtainedWith the f of accelerometer simulation output_bCalculate f_n, use Fourth order Runge-Kutta analog simulation Go out velocity error, that is, Kalman filter observed quantity；

(7) update after sytem matrix, observing matrix, sytem matrix and observing matrix are subjected to sliding-model control, brought into In Kalman equations, it is filtered；

(8) filtered quantity of state is preserved, exports simulation curve.

(9) last, according to systematic parameter simulation curve, estimating system parameter is simultaneously compensated.Before and after simulation data compensation The simulation estimate Comparative result curve map of velocity error and attitude error, analysis result is drawn a conclusion.

3rd, the simulation experiment result

Above-mentioned simulation result shows：

(1) ship, due to the impact of wave, produces oscillating motion in the process of navigation.The present invention can waved approximately Under conditions of, at 10 minutes or so with regard to part system error parameter can be calibrated, and without passing through turntable benchmark or artificial turn Dynamic, the excitation reasonably produced using the oscillating motion of ship can be achieved with the automatic Calibration of IMU systematic parameters.

(2) velocity error and attitude error after compensating are substantially better than the result before compensation.Therefore, demonstrate and carry in theory The ship self adaptation gone out, which waves scaling method, effectively on-line proving to go out systematic error parameter, reduce the mistake of inertial navigation system Difference, meets high accuracy FOG-IMU use demand, so as to improve the navigation accuracy of inertial navigation system.

(4), illustrate

Fig. 1 is that obtained accelerometer bias simulation estimate curve map is emulated using Matlab；

Fig. 2 is that obtained accelerometer constant multiplier simulation estimate curve map is emulated using Matlab；

Fig. 3 is to emulate obtained accelerometer using Matlab to install coefficient simulation estimate curve map；

Fig. 4 is that the obtained inclined simulation estimate curve map of gyro zero is emulated using Matlab；

Fig. 5 is that obtained gyro constant multiplier simulation estimate curve map is emulated using Matlab；

Fig. 6 is that obtained gyro installation coefficient simulation estimate curve map is emulated using Matlab；

Fig. 7 is velocity error simulation estimate Comparative result curve map before and after compensation；

Fig. 8 is attitude error simulation estimate Comparative result curve map before and after compensation.

(5), embodiment

Illustrate below and the present invention is described in more detail：

(1) initial alignment of systematic parameter is completed in laboratory, the related debugging of fiber-optic gyroscope strapdown inertial navigation system is completed Work.

(2) fiber-optic gyroscope strapdown inertial navigation system is arranged on ship center, after preheating, starts to gather the navigation of IMU outputs Data.

(3) determine initial state information to complete system coarse alignment according to IMU output, then determined according to coarse alignment Attitude information pass through a series of strapdowns resolve completion system fine alignment.

(4) inertia device input/output model is set up, it is then determined that the state equation and observational equation of Kalman filter.

(5) during ship's navigation, ship does approximate oscillating motion with wave, makes full use of it to produce inertia device Excitation.

(6) systematic error parameter is estimated and compensated again according to the Kalman filter equation of foundation, completes demarcation, Update systematic error parameter.

1) measurement model of inertia device is set up

Due to the limitation of mechanical processing technique, mounting condition equal error factor, a systems of accelerometer system and top in real system G systems of spiral shell system are not orthogonal coordinate system.According to accelerometer and the input/output relation of gyro, accelerometer and top are provided respectively Linear calibration's model of spiral shell passage.

The input and output Mathematical Modeling of accelerometer passage is expressed as：

A^a=δ K^af^b+δB^a+w^a

Wherein,Represent output of the accelerometer under a systems；Represent Force vector is compared in input of the accelerometer under carrier coordinate system b systems；Accelerometer biasSystem Random noiseK^aFor：

Wherein, constant multiplier of the accelerometer on i directions δ K_i(i=x, y, z) is represented, the accelerometer i under a systems The installation coefficient δ M between the measurement axle on the lower accelerometer j directions of measurement axle and b systems on direction_ijRepresent.

Laboratory Calibration is joined typically from high-accuracy turntable as benchmark, but in Kalman filter demarcation estimating system error Do not have turntable benchmark in several processes, so new constraint must be made, a kind of conventional method is defined in the orthogonal seats of IMU Mark system, i.e., under carrier coordinate system, X_bAxle and X_aDirection of principal axis is consistent, Y_bAxle is in O-X_aY_aIn plane, Z_bAxle is true according to the right-hand rule It is fixed.So δ K^aIt will be expressed as again：

Similarly, the Mathematical Modeling of gyro passage is represented by：

T^g=δ S^gω^b+δD^g+w^g

In formula,Represent output of three gyros under g systems；Represent Input angle speed of three gyros under b systems；The zero of three gyros is inclinedThe random gyro of system DriftS^gFor：

In above formula, constant multiplier of the gyro on i directions δ S_i(i=x, y, z) is represented, under g systems on gyro i directions Measurement axle and b systems time gyro j directions on measurement axle between installation coefficient δ E_ijRepresent.

2) state equation of Kalman filter is set up

Choose zero inclined, constant multiplier of velocity error, angular error, accelerometer and the gyro of inertial navigation system and system is installed Number is used as the state variable filtered.It is made up of altogether 27 state parameters：

Wherein,

Wherein, δ V_E、δV_NWith δ V_USystem east orientation, north orientation and sky orientation speed error, δ φ are represented respectively_E、δφ_NWith δ φ_UPoint Not Biao Shi system east orientation, north orientation and day to attitude error.

Using " east, north, day " geographic coordinate system as navigational coordinate system, according to SINS velocity errors and attitude error equations, Set up navigation error equation.Then it is described using linear first-order differential equation, draws the state equation of Kalman filter.

X is 27 dimension system state variableses, and F is 27 × 27 state matrixs for maintaining system, and G is the system noise square of 27 × 6 dimensions Battle array, W maintains system noise vector for 6, and 6 dimension noise vectors can be expressed as with white noise：

The noise transfer matrix of system can be usedTo represent.Wherein,With(i, j= 1,2,3) the strapdown transition matrix of accelerometer and gyro is represented respectively.

The writeable component block matrix form of state-transition matrix, is represented with F：Wherein：

R_eThe radius of the earth is represented, L represents the latitude in SINS locations, ω_ieRepresent earth rotation angular speed, V_EAnd V_NPoint East orientation speed and north orientation speed that Biao Shi be in strapdown solution process.f_E、f_NAnd f_URepresent respectively east orientation that accelerometer measures, North orientation and day are to output specific force.With(i, j=1,2,3) represents the strapdown transition matrix of accelerometer and gyro respectively.ω^b For real gyro angular speed input value.

3) observational equation for the Kalman filter set up：

During parameters revision, it is impossible to the accurate attitude error for obtaining carrier.And inertial navigation system is with Ship Swaying Line is not present in calibration process to move, angular movement is only existed, now the speed of navigation calculation is zero, and position is constant.Pass through speed Integration obtains the positional information of carrier, therefore using velocity error as observed quantity.

Z=[δ V_E δV_N δV_U]^T

Using IMU velocity error as observed quantity, then the measurement equation in filtering equations is represented by：

Z=HX+v

In above formula, H and v are respectively：

H=[diag { 111 } 0_3×24], v=[v_x v_y v_z]^T

Wherein, H represents observing matrix, and v, which is represented, measures noise.

4) error parameter is compensated

Result is modified using the method based on Kalman filter estimating system error parameter, FOG- can be obtained IMU systematic parameter High Definition Systems error parameter results.Afterwards, systematic parameter is compensated, is added to actual inertial navigation In system, it is determined that whether the parameter of demarcation can improve the precision of system.

The method compensated according to systematic error parameter, inertial navigation system velocity error is arrived by the compensation of smart Calibration Simulation estimated result In attitude error equations, velocity error and attitude error after being compensated, the error comparing result before and after output compensation.

Claims (4)

1. a kind of ship self adaptation based on Kalman filter waves scaling method, its feature comprises the following steps：

(1) initial alignment of systematic parameter is completed in laboratory, the related debugging efforts of fiber-optic gyroscope strapdown inertial navigation system are completed.

(2) fiber-optic gyroscope strapdown inertial navigation system is arranged on ship center, after preheating, starts to gather the navigation data of IMU outputs.

(3) determine initial state information to complete system coarse alignment according to IMU output, the appearance then determined according to coarse alignment State information resolves completion system fine alignment by a series of strapdowns.

(4) inertia device input/output model is set up, it is then determined that the state equation and observational equation of Kalman filter.

(5) during ship's navigation, ship does approximate oscillating motion with wave, makes full use of it to swash inertia device generation Encourage.

(6) systematic error parameter is estimated and compensated again according to the Kalman filter equation of foundation, completes demarcation, updated Systematic error parameter.

2. a kind of ship self adaptation based on Kalman filter according to the claim of right 1 waves scaling method, its feature It is：

(1) measurement model of accelerometer is expressed as：

A^a=δ K^af^b+δB^a+w^a

Wherein, A^aRepresent output of the accelerometer under a systems；f^bRepresent input specific force of the accelerometer under carrier coordinate system b systems Vector；Accelerometer bias δ B^a；The random noise w of system^a；Constant multiplier of the accelerometer on i directions δ K_i(i=x, y, Z) represent；The installation between measurement axle under measurement axle and b systems under a systems on accelerometer i directions on accelerometer j directions Coefficient δ M_ijRepresent.

(2) similarly, the measurement model of gyro is represented by：

T^g=δ S^gω^b+δD^g+w^g

In formula, T^gRepresent output of three gyros under g systems；ω^bRepresent input angle speed of three gyros under b systems；Three tops Zero inclined δ D of spiral shell^g；The random gyroscopic drift of systemConstant multiplier of the gyro on i directions δ S_i(i= X, y, z) represent；The installation coefficient between measurement axle under measurement axle and b systems under g systems on gyro i directions on gyro j directions Use δ E_ijRepresent.

3. a kind of ship self adaptation based on Kalman filter according to claims 1 waves scaling method, it is characterized in：

Choose zero inclined, constant multiplier of velocity error, angular error, accelerometer and the gyro of inertial navigation system and coefficient is installed and make For the state variable of filtering.

(1) Kalman filter state equation is set up

Using " east, north, day " geographic coordinate system as navigational coordinate system, according to SINS velocity errors and attitude error equations, set up Navigation error equation.Then it is described using linear first-order differential equation, draws the state equation of Kalman filter.

$\stackrel{\·}{X}=FX+GW$

X is 27 dimension system state variableses, and F is 27 × 27 state matrixs for maintaining system, and G is the system noise matrix of 27 × 6 dimensions, W System noise vector, 6 dimension noise vectors are maintained for 6.

The noise transfer matrix of system can be usedTo represent.Wherein,With The strapdown transition matrix of accelerometer and gyro is represented respectively.

The writeable component block matrix form of state-transition matrix, is represented with F：Wherein：

$F_{11}=\left[\begin{array}{cccccc}\frac{V_N}{R_e}\tan L& 2{\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R_e}\tan L& -(2{\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R_e})& 0& g& 0\\ -2{\mathrm{\ω}}_{ie}\sin L+\frac{2V_E}{R_e}\tan L& -\frac{V_U}{R_e}& -\frac{V_N}{R_e}& -g& 0& 0\\ 2{\mathrm{\ω}}_{ie}\cos L+\frac{2V_E}{R_e}& \frac{2V_N}{R_e}& 0& 0& 0& 0\end{array}\right],$

$F_{12}=\left[\begin{array}{ccccccccc}{c}_{11}^a& c_{12}^a& c_{13}^a& c_{11}^a{f}_x^b& c_{12}^a{f}_y^b& c_{13}^a{f}_z^b& c_{12}^a{f}_x^b& c_{13}^a{f}_x^b& c_{13}^a{f}_y^b\\ c_{21}^a& c_{22}^a& c_{23}^a& c_{21}^a{f}_x^b& c_{22}^a{f}_y^b& c_{23}^a{f}_z^b& c_{22}^a{f}_x^b& c_{23}^a{f}_x^b& c_{23}^a{f}_y^b\\ c_{31}^a& c_{32}^a& c_{33}^a& c_{31}^a{f}_x^b& c_{23}^a{f}_y^b& c_{33}^a{f}_z^b& c_{32}^a{f}_x^b& c_{33}^a{f}_x^b& c_{33}^a{f}_y^b\end{array}\right],$

$F_{21}=\left[\begin{array}{cccccc}0& -\frac{1}{R_e}& 0& 0& {\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R_e}\tan L& -{\mathrm{\ω}}_{ie}\cos L-\frac{V_E}{R_e}\\ \frac{1}{R_e}& 0& 0& -({\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R_e}\tan L)& 0& -\frac{V_N}{R_e}\\ \frac{1}{R_e}\tan L& 0& 0& {\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R_e}& \frac{V_N}{R_e}& 0\end{array}\right],$

$F_{23}=\left[\begin{array}{cccccccccccc}{c}_{11}^g& c_{12}^g& c_{13}^g& c_{11}^g{\mathrm{\ω}}_x^b& c_{12}^g{\mathrm{\ω}}_y^b& c_{13}^g{\mathrm{\ω}}_z^b& c_{11}^g{\mathrm{\ω}}_y^b& c_{11}^g{\mathrm{\ω}}_z^b& c_{11}^g{\mathrm{\ω}}_x^b& c_{12}^g{\mathrm{\ω}}_z^b& c_{13}^g{\mathrm{\ω}}_x^b& c_{13}^g{\mathrm{\ω}}_z^b\\ c_{21}^g& c_{22}^g& c_{23}^g& c_{21}^g{\mathrm{\ω}}_x^b& c_{22}^g{\mathrm{\ω}}_y^b& c_{23}^g{\mathrm{\ω}}_z^b& {\mathrm{\ω}}_{21}^g{\mathrm{\ω}}_y^b& c_{21}^g{\mathrm{\ω}}_z^b& c_{21}^g{\mathrm{\ω}}_x^b& c_{22}^g{\mathrm{\ω}}_z^b& c_{23}^g{\mathrm{\ω}}_x^b& c_{23}^g{\mathrm{\ω}}_z^b\\ c_{31}^g& c_{32}^g& c_{33}^g& c_{31}^g{\mathrm{\ω}}_x^b& c_{32}^g{\mathrm{\ω}}_y^b& c_{33}^g{\mathrm{\ω}}_z^b& c_{31}^g{\mathrm{\ω}}_y^b& c_{31}^g{\mathrm{\ω}}_z^b& c_{31}^g{\mathrm{\ω}}_x^b& c_{32}^g{\mathrm{\ω}}_z^b& c_{33}^g{\mathrm{\ω}}_x^b& c_{33}^g{\mathrm{\ω}}_z^b\end{array}\right].$

R_eThe radius of the earth is represented, L represents the latitude in SINS locations, ω_ieRepresent earth rotation angular speed, V_EAnd V_NDifference table Show the east orientation speed and north orientation speed in strapdown solution process.f_E、f_NAnd f_UEast orientation, north orientation that accelerometer is measured are represented respectively With day to output specific force.WithThe strapdown transition matrix of accelerometer and gyro is represented respectively.ω^bIt is true Real gyro angular speed input value.

(2) observational equation of Kalman filter is set up：

Using IMU velocity error as observed quantity, then the measurement equation in filtering equations is represented by：

Z=HX+v

In above formula, H and v are respectively：

H=[diag { 111 } 0_3×24], v=[v_x v_y v_z]^T

Wherein, H represents observing matrix, and v, which is represented, measures noise.

4. a kind of ship self adaptation based on Kalman filter according to the claim of right 1 waves scaling method and carries out error Parameter is compensated, it is characterized in that：

Result is modified using the method based on Kalman filter estimating system error parameter, FOG-IMU systems can be obtained System parameter High Definition Systems error parameter result.Afterwards, systematic parameter is compensated, is added to actual inertial navigation system In, it is determined that whether the parameter of demarcation can improve the precision of system.

$\left[\begin{array}{c}{\hat{f}}_x^b\\ {\hat{f}}_y^b\\ {\hat{f}}_z^b\end{array}\right]={\left[\begin{array}{ccc}1+{\mathrm{\δK}}_x& 0& 0\\ {\mathrm{\δM}}_{yx}& 1+{\mathrm{\δK}}_y& 0\\ {\mathrm{\δM}}_{zx}& {\mathrm{\δM}}_{zy}& 1+{\mathrm{\δK}}_z\end{array}\right]}^{-1}\·\left[\begin{array}{c}{A}_x^a-{\mathrm{\δB}}_x^a\\ A_y^a-{\mathrm{\δB}}_y^a\\ A_z^a-{\mathrm{\δB}}_z^a\end{array}\right]$

$\left[\begin{array}{c}{\widehat{\mathrm{\ω}}}_x^b\\ {\widehat{\mathrm{\ω}}}_y^b\\ {\widehat{\mathrm{\ω}}}_z^b\end{array}\right]={\left[\begin{array}{ccc}1+{\mathrm{\δS}}_x& {\mathrm{\δE}}_{xy}& {\mathrm{\δE}}_{xz}\\ {\mathrm{\δE}}_{yx}& 1+{\mathrm{\δS}}_y& {\mathrm{\δE}}_{yz}\\ {\mathrm{\δE}}_{zx}& {\mathrm{\δE}}_{zy}& 1+{\mathrm{\δS}}_z\end{array}\right]}^{-1}.\left[\begin{array}{c}{\mathrm{\ω}}_x^g-{\mathrm{\δD}}_x^g\\ {\mathrm{\ω}}_y^g-{\mathrm{\δB}}_y^a\\ {\mathrm{\ω}}_z^g-{\mathrm{\δB}}_z^a\end{array}\right]$

The method compensated according to systematic error parameter, inertial navigation system velocity error and appearance are arrived by the compensation of smart Calibration Simulation estimated result In state error equation, velocity error and attitude error after being compensated, the error comparing result before and after output compensation.