CN107655476A - Pedestrian's high accuracy foot navigation algorithm based on Multi-information acquisition compensation - Google Patents

Abstract

The invention discloses a pedestrian high-precision foot navigation algorithm based on multi-information fusion compensation, which collects the original measurement data of the gyroscope and the accelerometer in real time, the initial error compensation of the gyroscope is zero, and the error compensation of the accelerometer is also zero. The strapdown inertial navigation calculation algorithm is used for strapdown calculation; on the basis of the above, a Kalman filter correction model is constructed for filter correction, the state equation is consistent, and the measurement equation is dynamically adjusted with the multidimensional measurement information. Under information detection, the corresponding algorithm is constructed, the measurement equation is transformed in real time, the error of the navigation positioning result and the sensor error are estimated and corrected, and the error compensation of the gyroscope and the error compensation of the accelerometer are returned; finally, the navigation positioning result is output, that is, the position of the pedestrian, Speed and attitude: Low-precision consumer-grade sensor chips and magnetic sensors are used to solve the problem of high-precision pedestrian positioning and long-term heading divergence in a GNSS environment without a global satellite navigation and positioning system.

Description

Pedestrian high-precision foot navigation algorithm based on multi-information fusion compensation

technical field

The invention relates to a high-precision pedestrian foot navigation algorithm, in particular to a pedestrian high-precision foot navigation algorithm based on multi-information fusion compensation, which belongs to the technical field of pedestrian navigation.

Background technique

At present, research on pedestrian navigation technology is mainly divided into the following categories: wireless perception, pattern recognition, and inertial sensors. Among them, wireless sensing technology needs to deploy additional radio frequency equipment, including WIFI, UWB, etc., which has a narrow application range, easy to block signals, and high cost; while inertial sensors are independent, free from external interference, and low in cost, and are suitable for wide application. . However, the low-cost inertial sensor has a large error. If no compensation or other means are used to assist, it will cause errors above the meter level within a few seconds. This patent proposes to use the multi-information fusion compensation mechanism to design a zero-speed Kalman filter algorithm based on the inertial navigation system, to study the heading error self-observation algorithm of the pedestrian's initial static magnetic heading stability, and to study the zero-speed heading under the pedestrian's motion state The error self-observation algorithm, the fusion algorithm based on geomagnetic matching and strapdown inertial navigation algorithm, ensures high precision and high reliability when pedestrians are tall and long. This method is suitable for pedestrian navigation and solves high-precision pedestrian positioning in an environment without global satellite navigation and positioning system GNSS. Using low-cost sensors can realize pedestrian high-duration autonomous positioning, and has high engineering application and commercial value.

Contents of the invention

Purpose of the invention: The technical problem to be solved by this invention is to use low-precision inertial sensors and magnetic sensors to design a multi-information fusion compensation algorithm suitable for pedestrians' high-precision foot navigation.

Technical solutions:

A pedestrian high-precision foot navigation algorithm based on multi-information fusion compensation, which is characterized in that: the original measurement data of the gyroscope and the accelerometer are collected in real time, the initial error compensation of the gyroscope is zero, and the error compensation of the accelerometer is also zero, The strapdown inertial navigation calculation algorithm is used for strapdown calculation; on the basis of the above, a Kalman filter correction model is constructed for filter correction, the state equation is consistent, and the measurement equation is dynamically adjusted with the multidimensional measurement information. Under information detection, the corresponding algorithm is constructed, the measurement equation is transformed in real time, the error of the navigation positioning result and the sensor error are estimated and corrected, and the error compensation of the gyroscope and the error compensation of the accelerometer are returned; finally, the navigation positioning result is output, that is, the position of the pedestrian, speed, posture;

When only the zero-speed detection is successful, the pseudo-measurement information is used to construct a correction model algorithm based on the zero-speed Kalman filter; when the zero-speed detection is successful and the initial magnetic heading error self-observation is established, the magnetic heading error self-observation under the initial stationary state of pedestrians is constructed Algorithm; when the zero-speed detection is successful and the zero-speed heading error is self-observed in the dynamic state, the zero-speed heading error self-observation algorithm is constructed in the pedestrian motion state; when the zero-speed detection is successful, the zero-speed heading error is self-observed and For geomagnetic matching, a fusion algorithm based on geomagnetic matching and strapdown inertial navigation solution algorithm is constructed, in which the state equation and measurement equation of each algorithm work together to construct a Kalman filter correction model.

Further: the strapdown inertial navigation solution algorithm is as follows:

The inertial sensor is composed of an accelerometer and a gyroscope. The accelerometer obtains the specific force information of the moving carrier, and the velocity can be obtained through one integration, and the position can be obtained through the second integration; the gyroscope measures the angular velocity of the machine system relative to the inertial system, and through one integration The angle can be obtained; the most basic strapdown inertial navigation calculation algorithm includes attitude calculation, velocity calculation, and position calculation; therefore, the following formulas are used to calculate attitude, velocity, and position:

Among them, b is the body coordinate system, n is the navigation coordinate system, e is the earth coordinate system, i is the inertial coordinate system, f is the specific force, g is the gravitational acceleration, is the attitude transfer matrix from the body coordinate system to the navigation coordinate system, is the projection of the x, y, z three-axis machine system relative to the angular velocity of the inertial system on the machine system, is the projection of the angular velocity of the three-axis navigation system relative to the inertial system on the aircraft system, is the projection of the earth's rotation angular rate in the navigation system, is the column vector formed by the components of the angular velocity of the navigation coordinate system relative to the earth coordinate system in the axial direction of the navigation coordinate system, v is the three-axis velocity vector, and p is the three-axis position vector; for Derivative with respect to time t, is the derivative of v ⁿ with respect to time t, is the derivative of p ⁿ with respect to time t, so the above formula is an ordinary differential equation, which is solved iteratively on the basis of known initial attitude, velocity and position.

Further: the state equation of the Kalman filter correction model is as follows:

The Kalman filter model selects the "Northeast Sky" geographic coordinate system, and constructs an 18-order state model as follows:

Among them, δφ=[δφ _e δφ _n δφ _u ] is the platform angle error; δν=[δv _e δv _n δv _u ] is the speed error; δp=[δλ δL δh] is the position error, that is, the longitude, latitude and height error; ε _b = [ε _bx ε _by ε _bz ] is the random constant of the three-axis gyroscope; ε _r = [ε _rx ε _ry ε _rz ] is the first-order Markov process of the three-axis gyroscope, is the first-order Markov process of the three-axis accelerometer;

In different geographic coordinate systems The attitude transfer matrix is different, and the A, G, and W coefficients in the state equation are also different. In the "Northeast Sky" geographic coordinate system, the settings of A, G, and W are shown in the following four formulas: A is the system matrix, G is System noise matrix, W=[ω _gx ω _gy ω _gz ω _rx ω _ry ω _rz ω _ax ω _ay ω _az ] is the system noise, which is set according to the characteristics of the sensor itself; where ω _g is the random white noise drift of the three-axis gyroscope, its average The variance is σ _g , ω _r is the white noise driven by the Markov process with the mean square error of the three-axis gyroscope σ _r , and ω _a is the white noise driven by the Markov process with the mean square error of the three-axis accelerometer σ _a ;

is a matrix corresponding to nine basic navigation parameters, and T is the correlation time of the Markov process.

Further: the described correction model algorithm based on zero-speed Kalman filter is as follows:

When only the zero-speed detection is successful, use pseudo-measurement information to construct a six-dimensional measurement model in the zero-speed state; let veloP be a zero matrix of order 3×1, posiP(t)=posiN(t-1), then based on The zero-speed Kalman filter measurement model is shown in the following formula:

Among them, Z ₁ (t) is the observed quantity of velocity and position, veloN is the velocity calculated by the strapdown inertial navigation system in real time, posiN is the position calculated by the strapdown inertial navigation system in real time, and M _v is the velocity of the pedestrian at zero speed Error, M _p is the position error of the pedestrian at zero speed, V ₁ is the measurement noise, H ₁ is the measurement matrix, as shown in the following two formulas:

R _m is the radius of curvature of the earth in the meridian circle, R _n is the radius of curvature of the earth in the meridian circle, and L is the latitude.

Still further: the magnetic heading error self-observation algorithm in the initial static state of the pedestrian is as follows:

In the initial static state of pedestrians, the magnetic heading angle has strong stability. Therefore, when there is a successful zero-speed detection and self-observation of the initial magnetic heading error, a one-dimensional measurement equation is constructed, as shown in the following formula:

Among them, Z ₂ (t) is the observation of the initial heading angle, and ψ _INS is the heading angle calculated by the strapdown, which can be calculated by the strapdown inertial navigation algorithm The matrix is obtained by trigonometric function conversion, so that but ψ _mg is the magnetic heading angle, which can be calculated from the magnetic information measured by the magnetic sensor, is the magnetic heading angle solution error, V ₂ is the measurement noise;

In the initial static state of pedestrians, the following formula is used to further estimate the sensor error:

Z ₃ (t) is the observed quantity of velocity, position and initial heading angle.

Still further: the zero-speed heading error self-observation algorithm under the pedestrian motion state is as follows:

In the state of pedestrian movement, each step goes through a zero-speed moment, and the heading is basically unchanged in the zero-speed stage. Based on the theory of zero-speed heading invariance, when there is a successful zero-speed detection and zero-speed heading error self-observation in the dynamic state, record this The heading angle of the first zero-speed moment of the step is ψ _k-initial , and the heading angles of the remaining zero-speed moments of the step are ψ _k-others ;

The one-dimensional zero-speed heading self-observation measurement equation is constructed as follows:

Among them: Z ₄ (t) is the observed value of heading angle in motion state, is the heading angle error; is the zero-speed heading angle error, and V ₄ is the measurement noise; therefore:

θ is the pitch angle, which can be calculated by the strapdown inertial navigation algorithm The matrix is obtained by performing trigonometric function conversion,

Therefore, the 7-dimensional measurement model with zero-speed heading error self-observation algorithm in motion state can be obtained as follows:

Among them, Z ₅ (t) is the observed quantity of velocity, position and heading angle in the state of motion.

Further: the fusion algorithm based on geomagnetic matching and strapdown navigation solution algorithm is as follows:

When zero speed and geomagnetic matching are detected at the same time, a 10-dimensional measurement equation is constructed using a centralized filter to assist in correcting position error, speed error, and sensor error, as shown in the following three formulas, where posiM is the geomagnetic matching position, which can be determined by geomagnetic The matching is obtained directly, M ₆ is the geomagnetic matching position error, V ₆ is the measurement noise, posiN is the position calculated by the inertial navigation in real time, δp is the position error, and HI is shown in the correction model algorithm based on the zero-speed Kalman filter:

Z ₆ (t)=[posiN-posiM]=[δp+M ₆ ]=H ₆ (t)X(t)+V ₆ (t)

H ₆ ＝[0 _3×9 HI 0 _3×9 ]

Z ₆ (t) is the geomagnetic matching position observation, and Z ₇ (t) is the speed, position, heading angle in motion and geomagnetic matching position observation.

Beneficial effect:

1. Using low-precision consumer-grade sensor chips, it solves the problem of high-precision pedestrian positioning in a GNSS environment without a global satellite navigation and positioning system.

2. By using magnetic sensors, the problem of high long-term heading divergence is solved by using magnetic heading angle and geomagnetic matching.

3. Inertial sensors and magnetic sensors are low in cost and widely used, and the algorithm is more practical and popular.

Description of drawings

Figure 1 is a flowchart of pedestrian high-precision foot navigation algorithm based on multi-information fusion compensation.

Detailed ways

The present invention will be further explained below in conjunction with the accompanying drawings.

The present invention can be better understood from the following examples. However, those skilled in the art will readily understand that the specific material ratios, process conditions and results described in the examples are only used to illustrate the present invention, and should not and will not limit the present invention described in detail in the claims .

As shown in Figure 1, on the basis of the known initial position, velocity, and attitude of pedestrians, the information of accelerometer and gyroscope is collected in real time for strapdown inertial navigation calculation, and the velocity, position, and attitude of inertial navigation solution are obtained; In the zero-speed detection module, zero-speed detection is carried out and pseudo-measurement information is constructed. At the same time, the initial magnetic heading error self-observation, the zero-speed heading error self-observation and geomagnetic matching in the dynamic state are performed. When different conditions are established When constructing different Kalman filter measurement equations, the state equations are consistent; the position, velocity, and attitude errors estimated by the filter are compensated for the position, velocity, and attitude obtained by the strapdown inertial navigation system, and then the final navigation and positioning results are obtained. , that is, the position, speed, and attitude of the pedestrian; the sensor error estimated by filtering is returned to the accelerometer and gyroscope error compensation loop, and the sensor error is corrected in real time, and the cycle is repeated;

When only the zero-speed detection is successful, the pseudo-measurement information is used to construct a correction model algorithm based on the zero-speed Kalman filter; when the zero-speed detection is successful and the initial magnetic heading error self-observation is established, the magnetic heading error self-observation under the initial stationary state of pedestrians is constructed Algorithm; when the zero-speed detection is successful and the zero-speed heading error is self-observed in the dynamic state, the zero-speed heading error self-observation algorithm is constructed in the pedestrian motion state; when the zero-speed detection is successful, the zero-speed heading error is self-observed and For geomagnetic matching, a fusion algorithm based on geomagnetic matching and strapdown navigation algorithm is constructed, in which the state equation and measurement equation of each algorithm work together to construct a Kalman filter correction model.

The state equation of the Kalman filter correction model:

The Kalman filter model selects the "Northeast Sky" (ENU) geographic coordinate system, and constructs an 18-order state model as follows:

Among them, δφ=[δφ _e δφ _n δφ _u ] is the platform angle error; δν=[δv _e δv _n δv _u ] is the speed error; δp=[δλ δL δh] is the position error, that is, the longitude, latitude and height error; ε _b = [ε _bx ε _by ε _bz ] is the random constant of the three-axis gyroscope; ε _r = [ε _rx ε _ry ε _rz ] is the first-order Markov process of the three-axis gyroscope, is the first-order Markov process of the three-axis accelerometer;

in different coordinate systems The attitude transfer matrix is different, and the A, G, and W coefficients in the state equation are also different. In the "Northeast Sky" geographic coordinate system, the settings of A, G, and W are shown in the following four formulas: A is the system matrix, G is System noise matrix, W=[ω _gx ω _gy ω _gz ω _rx ω _ry ω _rz ω _ax ω _ay ω _az ] is the system noise, which is set according to the characteristics of the sensor itself; where ω _g is the random white noise drift of the three-axis gyroscope, its average The variance is σ _g , ω _r is the white noise driven by the Markov process with the mean square error of the three-axis gyroscope σ _r , and ω _a is the white noise driven by the Markov process with the mean square error of the three-axis accelerometer σ _a ;

is a matrix corresponding to 9 basic navigation parameters, and T is the correlation time of the Markov process;

Algorithm 1: Strapdown inertial navigation solution algorithm

The inertial navigation system is an autonomous navigation system that does not rely on any external information and does not radiate energy to the outside. It has the characteristics of good concealment and can work in various complex environments such as air, ground, and underwater. The main points are There are two types of platform inertial navigation systems and strapdown inertial navigation systems. The strapdown inertial navigation system is developed on the basis of the platform inertial navigation system, and it is a frameless system.

The inertial sensor is composed of an accelerometer and a gyroscope. The accelerometer and the gyroscope are directly connected to the carrier. The gyroscope and the accelerometer are used to measure the angular motion information and the linear motion information of the carrier respectively. Solve the heading, attitude, speed and position of the vehicle. The accelerometer obtains the specific force information of the moving carrier, the velocity can be obtained through one integration, and the position can be obtained through the second integration; the gyroscope measures the angular velocity of the machine system relative to the inertial system, and the angle can be obtained through one integration. The most basic strapdown inertial navigation calculation algorithm includes attitude calculation, velocity calculation, and position calculation. So the following formulas are used to calculate attitude, velocity and position:

Among them, b is the body coordinate system, n is the navigation coordinate system, e is the earth coordinate system, i is the inertial coordinate system, f is the specific force, g is the gravitational acceleration, is the attitude transfer matrix from the body coordinate system to the navigation coordinate system, is the projection of the x, y, z three-axis machine system relative to the angular velocity of the inertial system on the machine system, is the projection of the angular velocity of the three-axis navigation system relative to the inertial system on the aircraft system, is the projection of the earth's rotation angular rate in the navigation system, is the column vector formed by the components of the angular velocity of the navigation coordinate system relative to the earth coordinate system in the axial direction of the navigation coordinate system, v is the three-axis velocity vector, p is the three-axis position vector, for Derivative with respect to time t, is the derivative of v ⁿ with respect to time t, is the derivative of p ⁿ with respect to time t, so the above formula is an ordinary differential equation, which is solved iteratively on the basis of known initial attitude, velocity and position.

However, low-cost inertial sensors are noisy and can cause positioning errors above the meter level within a few seconds. Therefore, it is necessary to seek other additional auxiliary means to correct navigation positioning errors and suppress error divergence.

Algorithm 2: Kalman filter correction algorithm model based on zero speed

Construct a Kalman filter correction model based on zero speed, estimate sensor error and navigation positioning result error, and ensure the positioning accuracy when pedestrians are tall and long; when only zero speed detection is successful, use pseudo-measurement information to construct a six-dimensional quantity under zero speed state measurement model; let veloP be a 3×1 order zero matrix, posiP(t)=posiN(t-1), then the continuity equation of the zero-speed Kalman filter measurement model is as follows:

Among them, Z ₁ (t) is the observed quantity of velocity and position, veloN is the velocity calculated by the strapdown inertial navigation system in real time, posiN is the position calculated by the strapdown inertial navigation system in real time, and M _v is the velocity of the pedestrian at zero speed Error, M _p is the position error of the pedestrian at zero speed, V ₁ is the measurement noise, H ₁ is the measurement matrix, as shown in the following two formulas:

R _m is the radius of curvature of the earth in the meridian circle, R _n is the radius of curvature of the earth in the meridian circle, and L is the latitude.

Here, the position and speed virtual measurement information is used to assist in correcting sensor errors and navigation result errors, so as to maintain the accuracy of pedestrian navigation and positioning under high conditions. For the accuracy of the attitude, it is necessary to try to construct the heading-related measurement information to correct the heading error.

Algorithm 3: Magnetic heading error self-observation algorithm in the initial static state of pedestrians

The heading accuracy in pedestrian navigation and positioning is the main factor affecting pedestrian navigation and positioning, and the magnetic heading angle can be used as absolute heading information to correct the heading error. In the initial static state of pedestrians, the magnetic heading angle has strong stability. Therefore, when there is a successful zero-speed detection and self-observation of the initial magnetic heading error, a one-dimensional measurement equation is constructed, as shown in the following formula:

Among them, Z ₂ (t) is the observation of the initial heading angle, and ψ _INS is the heading angle calculated by the strapdown, which can be calculated by Algorithm 1 The matrix is obtained by trigonometric function conversion, so that but ψ _mg is the magnetic heading angle, which can be calculated from the magnetic information measured by the magnetic sensor, is the magnetic heading angle calculation error, and V ₂ is the measurement noise.

In the initial static state of pedestrians, the following formula is used to further estimate the sensor error:

Z ₃ (t) is the observed quantity of velocity, position and initial heading angle.

Algorithm 4: Self-observation Algorithm for Zero-speed Heading Error in Pedestrian Movement

In the state of pedestrian movement, each step goes through a zero-speed moment, and the heading is basically unchanged in the zero-speed stage. Based on the theory of zero-speed heading invariance, when there is a successful zero-speed detection and zero-speed heading error self-observation in the dynamic state, record this The heading angle at the first zero-speed moment of the step is ψ _k-initial , and the heading angles at the remaining zero-speed moments of the step are ψ _k-others .

The one-dimensional zero-speed heading self-observation measurement equation is constructed as follows:

Among them: Z ₄ (t) is the observed value of heading angle in motion state, is the heading angle error; is the zero-speed heading angle error, and V ₄ is the measurement noise; therefore:

θ is the pitch angle, which can be calculated by Algorithm 1 The matrix is obtained by performing trigonometric function conversion,

Therefore, the 7-dimensional measurement model with zero-speed heading error self-observation algorithm in motion state can be obtained as follows:

Among them, Z ₅ (t) is the observed quantity of velocity, position and heading angle in the state of motion.

Algorithm 5: Fusion Algorithm Based on Geomagnetic Matching and Strapdown Navigation Algorithm

Using geomagnetic matching information, the reliability of positioning is increased, and in the case of geomagnetic matching, the accuracy of pedestrian navigation and positioning can be further guaranteed. When zero speed and geomagnetic matching are detected at the same time, a 10-dimensional measurement equation is constructed using a centralized filter to assist in correcting position error, speed error, and sensor error, as shown in the following three formulas, where posiM is the geomagnetic matching position, which can be determined by geomagnetic The matching is obtained directly, M ₆ is the geomagnetic matching position error, V ₆ is the measurement noise, posiN is the position calculated by the inertial navigation in real time, δp is the position error, and HI is shown in Algorithm 2.

Z ₆ (t)=[posiN-posiM]=[δp+M ₆ ]=H ₆ (t)X(t)+V ₆ (t)

H ₆ ＝[0 _3×9 HI 0 _3×9 ]

Z ₆ (t) is the geomagnetic matching position observation, and Z ₇ (t) is the speed, position, heading angle in motion and geomagnetic matching position observation.

So far, a pedestrian high-precision foot navigation algorithm with multi-information fusion compensation has been constructed to ensure high-precision and high-duration pedestrian navigation. Among them, the algorithm 2345 actually uses multi-source information to improve the accuracy and fault tolerance of pedestrian navigation and positioning.

Algorithm 1 is the most basic strapdown inertial navigation calculation algorithm. For low-cost inertial sensors, the noise is relatively large. If it is not corrected, it will cause positioning errors above the meter level in a short time. Therefore, Algorithm 2 is used to use the position and speed virtual measurement information to assist in correcting sensor errors and navigation result errors, so as to maintain the accuracy of pedestrian navigation and positioning under high conditions. However, the Kalman filter in Algorithm 2 has low heading observability and It is closely related to the attitude, and it is necessary to try to construct the heading-related measurement information to correct the heading error; the magnetic heading angle can be used as the absolute heading information to correct the heading error. When the pedestrian is in the initial static state, the magnetic heading angle has a strong stability, so add algorithm 3, However, pedestrians do not necessarily have an initial static state, so add Algorithm 4 to improve heading observability, reduce heading errors, and further improve positioning accuracy; Algorithm 5 uses geomagnetic matching information to increase positioning reliability. In the case of geomagnetic matching , which can further guarantee the accuracy of pedestrian navigation and positioning.

The above is only a preferred embodiment of the present invention, it should be pointed out that, for those of ordinary skill in the art, without departing from the principle of the present invention, some improvements and modifications can also be made, and these improvements and modifications can also be made. It should be regarded as the protection scope of the present invention.

Claims (7)

1. A kind of 1. pedestrian's high accuracy foot navigation algorithm based on Multi-information acquisition compensation, it is characterised in that：Collection gyro in real time The raw measurement data of instrument and accelerometer, the error compensation of initial gyroscope is zero, and the error compensation of accelerometer is also zero, Strapdown resolving is carried out by strap-down inertial computation；On the basis of the above, structure Kalman filter correction model enters Row filter correction, state equation is consistent, and measurement equation adjusts with multidimensional measurement information dynamic, is detected in different multidimensional information Under, algorithm corresponding to structure, measurement equation real-time transform, estimation, amendment navigator fix resultant error and sensor error, return The error compensation of gyroscope and the error compensation of accelerometer；Finally export navigator fix result, i.e., the position of pedestrian, speed, Posture；

   When only zero-speed detection success, zero-speed Kalman filtering correction model algorithm is based on using pseudo- measurement information structure；When There are zero-speed detection success and initial magnetic heading error to be observed certainly from magnetic heading error under pedestrian's initial rest state when observing, is built Algorithm；, from when observing, zero-speed under pedestrian movement's state is built when there is zero-speed course error under zero-speed detection success and dynamical state Course error is from observation algorithm；When have zero-speed detection success, under motion state zero-speed course error from observing and during geomagnetic matching, The blending algorithm based on geomagnetic matching and strap-down inertial computation is built, wherein the state equation of each algorithm and measurement Equation collective effect constructs Kalman filter correction model.
2. 2. a kind of pedestrian's high accuracy foot navigation algorithm based on Multi-information acquisition compensation according to claim 1, it is special Sign is：The strap-down inertial computation is as follows：

   Inertial sensor is made up of accelerometer and gyroscope, and accelerometer obtains the ratio force information of motion carrier, by once Integration can obtain speed, and position can be obtained by quadratic integral；Gyroscope measuring machine system is fast relative to the angle of inertial system Degree, angle can be obtained by once integrating；Most basic strap-down inertial computation includes attitude algorithm, velocity solution Calculate, position resolves；Then following equation calculates posture, speed, position：

   <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <msubsup> <mover> <mi>C</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>b</mi> <mi>n</mi> </msubsup> <mo>=</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <msubsup> <mi>&amp;Omega;</mi> <mrow> <mi>i</mi> <mi>b</mi> </mrow> <mi>b</mi> </msubsup> <mo>-</mo> <msubsup> <mi>Q</mi> <mrow> <mi>i</mi> <mi>n</mi> </mrow> <mi>b</mi> </msubsup> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> </mtd> </mtr> <mtr> <mtd> <mrow> <msup> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>n</mi> </msup> <mo>=</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <msup> <mi>f</mi> <mi>b</mi> </msup> <mo>-</mo> <mo>&amp;lsqb;</mo> <mrow> <mo>(</mo> <mn>2</mn> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> <mi>n</mi> </msubsup> <mo>+</mo> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>e</mi> <mi>n</mi> </mrow> <mi>n</mi> </msubsup> <mo>)</mo> </mrow> <mo>&amp;times;</mo> <msup> <mi>v</mi> <mi>n</mi> </msup> <mo>-</mo> <mi>g</mi> <mo>&amp;rsqb;</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <msup> <mover> <mi>p</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>n</mi> </msup> <mo>=</mo> <msup> <mi>v</mi> <mi>n</mi> </msup> </mtd> </mtr> </mtable> </mfenced>

   Wherein, b is body axis system, and n is navigational coordinate system, and e is terrestrial coordinate system, and i is inertial coodinate system, and f is specific force, and g is Acceleration of gravity,For the posture transfer matrix of body axis system to navigational coordinate system,For x, y, z three, axle body system is relative In the projection that inertial system angular speed is fastened in body,Fastened for three spindle guides boat system relative to inertial system angular speed in body Projection,For earth rotation angular speed navigation be under projection,Angle for navigational coordinate system relative to terrestrial coordinate system The column vector that component of the speed in navigational coordinate system axial direction is formed, v is three axle speed vectors, and p is three shaft position vectors；ForOn time t derivative,For vⁿOn time t derivative,For pⁿOn time t derivative, therefore above formula is Chang Wei Divide equation, on the basis of known initial attitude, speed, position, iterative.
3. 3. a kind of pedestrian's high accuracy foot navigation algorithm based on Multi-information acquisition compensation according to claim 1, it is special Sign is that the state equation of the Kalman filtering correction model is as follows：

   Kalman filter model selects " northeast day " geographic coordinate system, and it is as follows to build 18 scalariform states models：

   <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mover> <mi>X</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <mi>A</mi> <mi>X</mi> <mo>+</mo> <mi>G</mi> <mi>W</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>X</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>&amp;delta;</mi> <mi>&amp;phi;</mi> </mrow> </mtd> <mtd> <mrow> <mi>&amp;delta;</mi> <mi>v</mi> </mrow> </mtd> <mtd> <mrow> <mi>&amp;delta;</mi> <mi>p</mi> </mrow> </mtd> <mtd> <msub> <mi>&amp;epsiv;</mi> <mi>b</mi> </msub> </mtd> <mtd> <msub> <mi>&amp;epsiv;</mi> <mi>r</mi> </msub> </mtd> <mtd> <msub> <mo>&amp;dtri;</mo> <mi>r</mi> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> </mtable> </mfenced>

   Wherein, δ φ=[δ φ_e δφ_n δφ_u] it is platform angle error；δ ν=[δ v_e δv_n δv_u] it is velocity error；δ p=[δ λ δ L δ h] it is that site error is longitude, latitude, height error；ε_b=[ε_bx ε_by ε_bz] it is three axis accelerometer arbitrary constant；ε_r= [ε_rx ε_ry ε_rz] it is three-axis gyroscope single order markoff process, ▽_r=[▽_rx ▽_ry ▽_rz] it is three axis accelerometer single order Markoff process；

   Under different geographic coordinate systemsPosture transfer matrix is different, and the A, G, W coefficient in state equation are also different, in " east Under northern day " geographic coordinate system, A, G, W are set as shown in following four formula：A is sytem matrix, and G is system noise matrix, W=[ω_gx ω_gy ω_gz ω_rx ω_ry ω_rz ω_ax ω_ay ω_az] it is system noise, set according to sensor self character；Wherein ω_gFor Three axis accelerometer random white noise drifts about, and its mean square deviation is σ_g, ω_rIt is that three axis accelerometer mean square deviation is σ_rMarkoff process driving White noise, ω_aIt is that three axis accelerometer mean square deviation is σ_aMarkoff process driving white noise；

   <mrow> <mi>G</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mo>-</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> </mrow> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mn>9</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>9</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>9</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mi>I</mi> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mi>I</mi> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow>

   <mrow> <mi>A</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mrow> <mo>(</mo> <msub> <mi>A</mi> <mrow> <mi>I</mi> <mi>N</mi> <mi>S</mi> </mrow> </msub> <mo>)</mo> </mrow> <mrow> <mn>9</mn> <mo>&amp;times;</mo> <mn>9</mn> </mrow> </msub> </mtd> <mtd> <msub> <mrow> <mo>(</mo> <msub> <mi>A</mi> <mrow> <mi>s</mi> <mi>g</mi> </mrow> </msub> <mo>)</mo> </mrow> <mrow> <mn>9</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mn>9</mn> <mo>&amp;times;</mo> <mn>9</mn> </mrow> </msub> </mtd> <mtd> <msub> <mrow> <mo>(</mo> <msub> <mi>A</mi> <mrow> <mi>I</mi> <mi>M</mi> <mi>U</mi> </mrow> </msub> <mo>)</mo> </mrow> <mrow> <mn>9</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow>

   <mrow> <msub> <mi>A</mi> <mrow> <mi>I</mi> <mi>M</mi> <mi>U</mi> </mrow> </msub> <mo>=</mo> <mi>d</mi> <mi>i</mi> <mi>a</mi> <mi>g</mi> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <mfrac> <mn>1</mn> <msub> <mi>T</mi> <mrow> <mi>r</mi> <mi>x</mi> </mrow> </msub> </mfrac> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mfrac> <mn>1</mn> <msub> <mi>T</mi> <mrow> <mi>r</mi> <mi>y</mi> </mrow> </msub> </mfrac> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mfrac> <mn>1</mn> <msub> <mi>T</mi> <mrow> <mi>r</mi> <mi>z</mi> </mrow> </msub> </mfrac> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mfrac> <mn>1</mn> <msub> <mi>T</mi> <mrow> <mi>a</mi> <mi>x</mi> </mrow> </msub> </mfrac> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mfrac> <mn>1</mn> <msub> <mi>T</mi> <mrow> <mi>a</mi> <mi>y</mi> </mrow> </msub> </mfrac> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mfrac> <mn>1</mn> <msub> <mi>T</mi> <mrow> <mi>a</mi> <mi>z</mi> </mrow> </msub> </mfrac> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>

   <mrow> <msub> <mi>A</mi> <mrow> <mi>s</mi> <mi>g</mi> </mrow> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mo>-</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> </mrow> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> </mtd> </mtr> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow>

   It is the matrix of corresponding 9 basic navigation parameters, when T is the correlation of markoff process Between.
4. 4. a kind of pedestrian's high accuracy foot navigation algorithm based on Multi-information acquisition compensation according to claim 1, it is special Sign is, described as follows based on zero-speed Kalman filtering correction model algorithm：

   In the success of only zero-speed detection, the sextuple measurement model under zero-speed state is built using pseudo- measurement information；The veloP is made to be The null matrix of 3 × 1 ranks, posiP (t)=posiN (t-1), then the Kalman filtering measurement model based on zero-speed be shown below：

   <mrow> <msub> <mi>Z</mi> <mn>1</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mo>&amp;lsqb;</mo> <mtable> <mtr> <mtd> <mrow> <mi>v</mi> <mi>e</mi> <mi>l</mi> <mi>o</mi> <mi>N</mi> <mo>-</mo> <mi>v</mi> <mi>e</mi> <mi>l</mi> <mi>o</mi> <mi>P</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>p</mi> <mi>o</mi> <mi>s</mi> <mi>i</mi> <mi>N</mi> <mo>-</mo> <mi>p</mi> <mi>o</mi> <mi>s</mi> <mi>i</mi> <mi>P</mi> </mrow> </mtd> </mtr> </mtable> <mo>&amp;rsqb;</mo> <mo>=</mo> <mo>&amp;lsqb;</mo> <mtable> <mtr> <mtd> <mrow> <mi>&amp;delta;</mi> <mi>v</mi> <mo>+</mo> <msub> <mi>M</mi> <mi>v</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>&amp;delta;</mi> <mi>p</mi> <mo>+</mo> <msub> <mi>M</mi> <mi>p</mi> </msub> </mrow> </mtd> </mtr> </mtable> <mo>&amp;rsqb;</mo> <mo>=</mo> <msub> <mi>H</mi> <mn>1</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mi>X</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>V</mi> <mn>1</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow>

   Wherein, Z₁(t) it is the observed quantity of speed and position, veloN is the speed of SINS real-time resolving, and posiN is victory Join the position of inertial navigation system real-time resolving, M_vFor velocity error under pedestrian's zero-speed state, M_pMissed for position under pedestrian's zero-speed state Difference, V₁To measure noise, H₁For measurement matrix, as shown in following two formula：

   <mrow> <msub> <mi>H</mi> <mn>1</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mi>I</mi> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>12</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>6</mn> </mrow> </msub> </mtd> <mtd> <mrow> <mi>H</mi> <mi>I</mi> </mrow> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>9</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow>

   <mrow> <mi>H</mi> <mi>I</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>R</mi> <mi>m</mi> </msub> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msub> <mi>R</mi> <mi>n</mi> </msub> <mi>cos</mi> <mi> </mi> <mi>L</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> </mrow>

   R_mFor meridian circle radius of curvature of the earth, R_nFor prime vertical radius of curvature of the earth, L is latitude.
5. 5. a kind of pedestrian's high accuracy foot navigation algorithm based on Multi-information acquisition compensation according to claim 4, it is special Sign is：Magnetic heading error is as follows from observation algorithm under pedestrian's initial rest state：

   Under pedestrian's initial rest state, magnetic heading angle has stronger stability, therefore working as has zero-speed detection successful and initial magnetic Course error builds one-dimensional measurement equation, is shown below from when observing：

   Wherein, Z₂(t) it is the observed quantity at initial heading angle, ψ_INSThe course angle resolved for strapdown, can be resolved by strap-down inertial In algorithmMatrix carries out trigonometric function conversion and obtained, orderThenψ_mgFor Magnetic heading angle, it can be resolved and obtained by the magnetic information that Magnetic Sensor measurement obtains,For magnetic heading angle resolution error, V₂Made an uproar to measure Sound；

   Under pedestrian's initial rest state, the further estimation to sensor error is realized using following formula：

   <mrow> <msub> <mi>Z</mi> <mn>3</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mo>&amp;lsqb;</mo> <mtable> <mtr> <mtd> <mrow> <msub> <mi>Z</mi> <mn>1</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>Z</mi> <mn>2</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> <mo>&amp;rsqb;</mo> </mrow>

   Z₃(t) it is the observed quantity of speed, position and initial heading angle.
6. 6. a kind of pedestrian's high accuracy foot navigation algorithm based on Multi-information acquisition compensation according to claim 4, it is special Sign is：Zero-speed course error is as follows from observation algorithm under pedestrian movement's state：

   Under pedestrian movement's state, each step undergoes one section of zero-speed moment, the zero-speed stage course be basically unchanged, based on zero-speed course Constant theory, from when observing, remember the step the first zero-speed moment when there is zero-speed course error under zero-speed detection success and dynamical state The course angle of point is ψ_k-initial, the course angle for remembering remaining zero-speed moment point of the step is ψ_k-others；

   It is as follows from measurement equation is observed to build one-dimensional zero-speed course：

   Wherein：Z₄(t) it is the observed quantity of course angle under motion state,For course angle error；For zero-speed course angle error, V₄ To measure noise；Therefore：

   θ is the angle of pitch, can be by strap-down inertial computationMatrix carries out trigonometric function conversion and obtained,

   Then addition zero-speed course error under motion state can be obtained is from 7 dimension measurement models of observation algorithm：

   <mrow> <msub> <mi>Z</mi> <mn>5</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mo>&amp;lsqb;</mo> <mtable> <mtr> <mtd> <mrow> <msub> <mi>Z</mi> <mn>1</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>Z</mi> <mn>4</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> <mo>&amp;rsqb;</mo> </mrow>

   Wherein, Z₅(t) be speed, under position and motion state course angle observed quantity.
7. 7. a kind of pedestrian's high accuracy foot navigation algorithm based on Multi-information acquisition compensation according to claim 4, it is special Sign is：The blending algorithm based on geomagnetic matching and strap-down navigation computation is as follows：

   When being detected simultaneously by zero-speed and geomagnetic matching success, the dimension measurement equation of concentrated filter structure 10, auxiliary amendment are used Site error, velocity error and sensor error, as shown in following three formula, wherein posiM is geomagnetic matching position, can be by earth magnetism Matching directly obtains, M₆For geomagnetic matching site error, V₆To measure noise, posiN is the position of inertial navigation real-time resolving, and δ p are Site error, HI is based on as shown in zero-speed Kalman filtering correction model algorithm：

   Z₆(t)=[posiN-posiM]=[δ p+M₆]=H₆(t)X(t)+V₆(t)

   H₆=[0_3×9 HI 0_3×9]

   <mrow> <msub> <mi>Z</mi> <mn>7</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mo>&amp;lsqb;</mo> <mtable> <mtr> <mtd> <mrow> <msub> <mi>Z</mi> <mn>1</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>Z</mi> <mn>4</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>Z</mi> <mn>6</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> <mo>&amp;rsqb;</mo> </mrow>

   Z₆(t) it is geomagnetic matching position detection amount, Z₇(t) it is course angle and geomagnetic matching position under speed, position, motion state Put observed quantity.