CN106017461A - Pedestrian navigation system three-dimensional spatial positioning method based on human/environment constraints - Google Patents

Abstract

The invention relates to a pedestrian navigation system three-dimensional spatial positioning method based on human/environment constraints and solves the problem with high-precision positioning of a pedestrian navigation system in a three-dimensional space, wherein the system is designed based on a low-cost MEMS (micro-electromechanical system) inertial measurement unit; data collection and strap-down navigation calculation are performed by installing the MEMS inertial measurement unit on a shoe, and zero-speed ranges in pedestrian gaits are detected in connection with data output by a pressure sensor installed on a sole. A scheme is provided based on zero speed correction in connection with pedestrian motion characteristics and ambient building characteristics, the scheme is about constraining magnetic field abnormality in zero-speed ranges and constraining SINS (ship's inertial navigation system) height channel divergence, and an extended Kalman filter is designed to correct errors in navigation calculation results, thereby enabling high-precision positioning of pedestrians in the three-dimensional space.

Description

Pedestrian navigation system three-dimensional fix method based on human body/environmental constraints

Technical field

The invention belongs to technical field of navigation and positioning, it particularly relates to a kind of pedestrian based on human body/environmental constraints leads Boat system three-dimensional fix method.

Background technology

Pedestrian navigation system can help the position of personnel's real-time positioning oneself of execution task and obtain with command centre Contact, thus greatly ensure ambulance paramedic's life security in circumstances not known.Additionally, on airport, theater, underground stop In the large-scale public places such as parking lot and built-up modern city, high, the real-time pedestrian navigation system of precision can Effectively help pedestrian to carry out positioning oneself and target is looked for.But existing global position system is defended in urban canyons, indoor etc. The environment of star signal limitation or inefficacy cannot normally work, thus the most autonomous location cannot be realized.Therefore, carry out without defending Pedestrian navigation location technology under star signal is with a wide range of applications.

Along with the development of MEMS technology, MEMS sensor has low cost, lightweight, size is little, the advantage such as low in energy consumption, with Time have in view of inertial navigation that autonomy is strong, the feature of strong interference immunity, foot installs the pedestrian of MEMS inertial sensor and leads Boat system becomes current study hotspot.But MEMS sensor general precision is relatively low, calculate navigation ginseng using SINS algorithm During number, the site error that in integral process, acceleration error causes can cube in time increase.

Therefore, the precision of location how is improved, it has also become urgent problem now.

Summary of the invention

For solving the problems referred to above, the invention provides a kind of pedestrian navigation system three-dimensional space based on human body/environmental constraints Between localization method, the method comprises the steps:

S101, installs MEMS Inertial Measurement Unit, installs pressure transducer at sole heel at heel position；

S102, gathers MEMS Inertial Measurement Unit and pressure transducer output data, extracts zero in pedestrian movement's gait Speed is interval；The formula in the concrete zero-speed interval extracted in pedestrian movement's gait is:

a_min＜ a_x(k)²+a_y(k)²+a_z(k)²＜ a_max

$\frac{1}{n-1}\underset{k=i}{\overset{i+n-1}{\&Sigma;}}{(a_k-{\stackrel{\&OverBar;}{a}}_n)}^2<T_{A\_{\mathrm{\&sigma;}}_a}$

f(k)-T_f＞ 0

|ω_y(k) | ＜ T_ω

If above four Rule of judgment are set up simultaneously, then it is assumed that current time pedestrian foot remains static；

Wherein, a_iK () represents that k moment i axially goes up accelerometer output, i=x, y, z.a_min、a_maxIt is threshold interval respectively Left margin and right margin；a_kIt is accelerometer output,Being the meansigma methods of data point in window, n represents window width, For variance threshold values；The force value that f (k) the expression k moment detects, T_fRepresent pressure threshold；|ω_y(k) | represent k moment y-axis gyro The absolute value of output, T_ωFor angular speed threshold value；

S103, utilizes acceleration, angular velocity information that in step S102, MEMS Inertial Measurement Unit is measured, and below combination Formula carries out strap-down navigation resolving, obtains the position of pedestrian, speed, attitude information；

$\stackrel{\&CenterDot;}{Q}=\frac{1}{2}({\mathrm{\&omega;}}_{nb}^b\&times;)Q$

${\stackrel{\&CenterDot;}{V}}^n=C_b^n{f}^b-(2{\mathrm{\&omega;}}_{ie}^n+{\mathrm{\&omega;}}_{en}^n)\&times;V^n+g^n$

${\stackrel{\&CenterDot;}{P}}^n=V^n$

Wherein, subscript n represents that navigational coordinate system, b represent carrier coordinate system.Q represents that attitude quaternion, V represent speed, P Represent position.It is the transmission matrix being tied to n system by b,Represent the antisymmetry square that the angular velocity measured by gyroscope is constituted Battle array, f^bRepresent specific force,Represent rotational-angular velocity of the earth,Represent that navigation is the angle of rotation speed of relative earth system, gⁿRepresent Terrestrial gravitation field vector, can be approximately constant value in the most a certain scope；

S104, utilizes the zero-speed interval extracted in step S102 to carry out Kalman filtering, estimates that pedestrian system state is by mistake SINS calculation result is also modified by difference；

System Kalman filter model is:

δx_k+1=Φ_kδx_k+w_k

δz_k=H_kδx_k+v_k

Wherein, δ x_kIt is the system mode in k moment, Φ_kIt is state-transition matrix, w_kRepresent process noise, δ z_kFor the k moment Error observed quantity, H_kFor calculation matrix, v_kFor measuring noise；

Definition error state vector is:

δ x=[δ φⁿ δVⁿ δPⁿ ε^b ▽^b]

Wherein comprising 9 navigation errors and 6 sensor errors in error state vector, they are three-dimensional position respectively Error vector δ φⁿ, velocity error vector δ Vⁿ, attitude vectors δ Pⁿ, gyro biased error vector ε^bAnd add table biased error vector ▽^b；

State-transition matrix is:

${\mathrm{\&Phi;}}_k=\left[\begin{array}{ccccc}{I}_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& \mathrm{\&Delta;}t\&CenterDot;C_b^n& 0_{3\&times;3}\\ \mathrm{\&Delta;}t\&CenterDot;f^n\&times;& I_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& \mathrm{\&Delta;}t\&CenterDot;C_b^n\\ 0_{3\&times;3}& \mathrm{\&Delta;}t\&CenterDot;I_{3\&times;3}& I_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& \left(1-\frac{\mathrm{\&Delta;}t}{{\mathrm{\&tau;}}_g}\right)I_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& \left(1-\frac{\mathrm{\&Delta;}t}{{\mathrm{\&tau;}}_a}\right)I_{3\&times;3}\end{array}\right]$

Wherein, Δ t represents sampling time interval, fⁿ× under navigational coordinate system than force component constitute antisymmetric matrix；

In zero-speed interval, data noise expression formula is:

Δ V=V_INS-V_Stance

The measurement matrix of zero-velocity curve is:

H_zupt=[0_3×3 I_3×3 0_3×3 0_3×3 0_3×3]

S105, while performing step S104, utilizes the output of Magnetic Sensor to calculate each zero-speed in zero-speed interval The course angle that point is corresponding, in step S104 goes to zero-speed interval during last point of zero velocity, senses magnetic in zero-speed interval All course angles that device calculates make smothing filtering, and result are exported as observed quantity and carry out course to Kalman filter Revise；

S106, while performing step S104, it is judged that the height change characteristic of pedestrian under environmental constraint, the most more New high degree observed quantity also outputs it to Kalman filter and carries out height correction；

Perform can obtain revised pedestrian navigation parameter in real time by the circulation of step S102 to S106.

Further, above-mentioned MEMS Inertial Measurement Unit, comprise 3 axis MEMS accelerometer, three axis MEMS gyro and three Axle MEMS Magnetic Sensor.

Further, in described step S105: while performing step S104, the output of Magnetic Sensor is utilized to calculate zero The course angle that in speed is interval, each point of zero velocity is corresponding, in step S104 goes to zero-speed interval during last point of zero velocity, The all course angles calculating Magnetic Sensor in zero-speed interval make smothing filtering, and export result to Kalman filtering Device；In zero-speed interval, the calculation expression of course angle and course angle measurement error is:

${\mathrm{Heading}}_{total}=\underset{i=1}{\overset{n-1}{\&Sigma;}}{\mathrm{Heading}}_{Mag}\left(i\right)$

Heading=(Heading_total+Heading_Mag(n))/n

Wherein, n represents the number of point of zero velocity, Heading in zero-speed interval_totalRepresent remove in zero-speed interval last zero The magnetic heading angle summation that speed point other point of zero velocity outer is corresponding, Heading_MagI () represents zero-speed interval in, i-th point of zero velocity is corresponding Magnetic heading angle, all course angles that in Heading zero-speed interval, Magnetic Sensor calculates make the value of smothing filtering,Represent boat To angle error,Represent the calculated course angle of SINS.

The measurement matrix of navigational calibration is:

H_heading=[[0 0 1] 0_3×3 0_3×3 0_3×3 0_3×3]

Further, in described step S106: while performing step S104, it is judged that under by environmental constraint Pedestrian level variation characteristic, real-time update height observed quantity also outputs it to Kalman filter；Stair are utilized to top bar height When degree retrains, need to first extract the zero-speed in gait interval, height between adjacent point of zero velocity in then judging zero-speed interval Relation between changing value and setting height threshold value, identifies the kinestate of pedestrian；The elevation information that definition k moment SINS resolves is h_INS, the k-1 moment update after elevation information be h_prev, the difference in height of the two is designated as Δ h=h_INS-h_prev, height threshold is designated as th_h, the k moment update after elevation information be h_cuur, now the relation between the height value of navigation calculation and the height value of observation can table Reach for:

Δ H=h_INS-h_cuur

The measurement matrix of height correction is:

H_height=[0_3×3 0_3×3 [0 0 1] 0_3×3 0_3×3]。

Compared with prior art, pedestrian navigation system three dimensions based on human body/environmental constraints of the present invention is fixed Method for position has the advantage that

(1) present invention is on the basis of utilizing MEMS Inertial Measurement Unit output data to carry out the detection of zero-speed interval, introduces It is interval that pressure transducer auxiliary MIMU extracts the zero-speed in gait.Output characteristics based on pressure transducer, multi-sensor data The mode of output combination significantly reduces the interval flase drop of zero-speed and loss, can be effectively improved the resolving of pedestrian navigation system Precision.

(2) present invention combines pedestrian movement's feature, and the course angle calculating Magnetic Sensor in zero-speed interval makees smothing filtering, Reduce the abnormal course error caused in magnetic field at the point of zero velocity of local, thus improve reliable on the basis of ensureing navigational calibration Property.

(3) method of the present invention considers the problem that SINS solution process camber passage dissipates, in combination with row People's motor process exists inside and outside multi-story structure in surrounding stair and stair topped bar highly consistent characteristic, it is proposed that Utilize stair size to carry out highly constrained method, improve pedestrian system positioning precision in three dimensions.

Accompanying drawing explanation

Fig. 1 is pedestrian navigation system three-dimensional fix method flow diagram based on human body/environmental constraints.

Fig. 2 is based on the navigation calculation flow chart of course angle constraint in zero-speed interval.

Fig. 3 is the navigation calculation flow chart highly constrained based on stairway step in environment.

Detailed description of the invention

Below by specific embodiment, the invention will be further described, and following example are illustrative, is not limit Qualitatively, it is impossible to limit protection scope of the present invention with this.

Determine as it is shown in figure 1, the present invention is claimed a kind of pedestrian navigation system three dimensions based on human body/environmental constraints Method for position, the method includes the steps of:

S101, installs MEMS (Micro-Electro-Mechanical System) Inertial Measurement Unit at heel position, Before MIMU coordinate axes is respectively directed to human body, direction right, lower；At sole heel, pressure transducer is installed, it is ensured that it is sensitive Axle is towards vertical direction；

Above-mentioned MEMS Inertial Measurement Unit, comprises 3 axis MEMS accelerometer, three axis MEMS gyro and 3 axis MEMS magnetic Sensor；

S102, gathers MEMS Inertial Measurement Unit and pressure transducer output data, extracts zero in pedestrian movement's gait Speed is interval；The formula in the concrete zero-speed interval extracted in pedestrian movement's gait is:

a_min＜ a_x(k)²+a_y(k)²+a_z(k)²＜ a_max

$\frac{1}{n-1}\underset{k=i}{\overset{i+n-1}{\&Sigma;}}{(a_k-{\stackrel{\&OverBar;}{a}}_n)}^2<T_{A\_{\mathrm{\&sigma;}}_a}$

f(k)-T_f＞ 0

|ω_y(k) | ＜ T_ω

If above four Rule of judgment are set up simultaneously, then it is assumed that current time pedestrian foot remains static；

Wherein, a_iK () represents that k moment i axially goes up accelerometer output, i=x, y, z.a_min、a_maxIt is threshold interval respectively Left margin and right margin；a_kIt is accelerometer output,Being the meansigma methods of data point in window, n represents window width,For variance threshold values；The force value that f (k) the expression k moment detects, T_fRepresent pressure threshold；|ω_y(k) | represent k moment y The absolute value of axle gyro output, T_ωFor angular speed threshold value；

S103, utilizes acceleration, angular velocity information that in step S102, MEMS Inertial Measurement Unit is measured, and below combination Formula carries out strap-down navigation resolving, obtains the position of pedestrian, speed, attitude information；

$\stackrel{\&CenterDot;}{Q}=\frac{1}{2}({\mathrm{\&omega;}}_{nb}^b\&times;)Q$

${\stackrel{\&CenterDot;}{V}}^n=C_b^n{f}^b-(2{\mathrm{\&omega;}}_{ie}^n+{\mathrm{\&omega;}}_{en}^n)\&times;V^n+g^n$

${\stackrel{\&CenterDot;}{P}}^n=V^n$

Wherein, subscript n represents that navigational coordinate system, b represent carrier coordinate system.Q represents that attitude quaternion, V represent speed, P Represent position.It is the transmission matrix being tied to n system by b,Represent the antisymmetry that the angular velocity measured by gyroscope is constituted Matrix, f^bRepresent specific force,Represent rotational-angular velocity of the earth,Represent that navigation is the angle of rotation speed of relative earth system, gⁿTable Show terrestrial gravitation field vector, constant value in the most a certain scope, can be approximately.

S104, utilizes the zero-speed interval extracted in step S102 to carry out Kalman filtering, estimates that pedestrian system state is by mistake SINS calculation result is also modified by difference；

System Kalman filter model is:

δx_k+1=Φ_kδx_k+w_k

δz_k=H_kδx_k+v_k

Wherein, δ x_kIt is the system mode in k moment, Φ_kIt is state-transition matrix, w_kRepresent process noise, δ z_kFor the k moment Error observed quantity, H_kFor calculation matrix, v_kFor measuring noise；

Definition error state vector is:

δ x=[δ φⁿ δVⁿ δPⁿ ε^b ▽^b]

Wherein comprising 9 navigation errors and 6 sensor errors in error state vector, they are three-dimensional position respectively Error vector δ φⁿ, velocity error vector δ Vⁿ, attitude vectors δ Pⁿ, gyro biased error vector ε^bAnd add table biased error vector ▽^b。

State-transition matrix is:

${\mathrm{\&Phi;}}_k=\left[\begin{array}{ccccc}{I}_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& \mathrm{\&Delta;}t\&CenterDot;C_b^n& 0_{3\&times;3}\\ \mathrm{\&Delta;}t\&CenterDot;f^n\&times;& I_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& \mathrm{\&Delta;}t\&CenterDot;C_b^n\\ 0_{3\&times;3}& \mathrm{\&Delta;}t\&CenterDot;I_{3\&times;3}& I_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& \left(1-\frac{\mathrm{\&Delta;}t}{{\mathrm{\&tau;}}_g}\right)I_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& \left(1-\frac{\mathrm{\&Delta;}t}{{\mathrm{\&tau;}}_a}\right)I_{3\&times;3}\end{array}\right]$

Wherein, Δ t represents sampling time interval, fⁿ× under navigational coordinate system than force component constitute antisymmetric matrix.

In zero-speed interval, data noise expression formula is:

Δ V=V_INS-V_Stance

The measurement matrix of zero-velocity curve is:

H_zupt=[0_3×3 I_3×3 0_3×3 0_3×3 0_3×3]

S105, while performing step S104, utilizes the output of Magnetic Sensor to calculate each zero-speed in zero-speed interval The course angle that point is corresponding, in step S104 goes to zero-speed interval during last point of zero velocity, senses magnetic in zero-speed interval All course angles that device calculates make smothing filtering, and result are exported and carry out navigational calibration to Kalman filter；

Fig. 2 is based on the navigation calculation flow chart of course angle constraint, course angle and course in zero-speed interval in zero-speed interval The calculation expression of measurement error is:

${\mathrm{Heading}}_{total}=\underset{i=1}{\overset{n-1}{\&Sigma;}}{\mathrm{Heading}}_{Mag}\left(i\right)$

Heading=(Heading_total+Heading_Mag(n))/n

Wherein, n represents the number of point of zero velocity, Heading in zero-speed interval_totalRepresent remove in zero-speed interval last zero The magnetic heading angle summation that speed point other point of zero velocity outer is corresponding, Heading_MagI () represents zero-speed interval in, i-th point of zero velocity is corresponding Magnetic heading angle, all course angles that in Heading zero-speed interval, Magnetic Sensor calculates make the value of smothing filtering,Represent boat To angle error,Represent the calculated course angle of SINS.

The measurement matrix of navigational calibration is:

H_heading=[[0 0 1] 0_3×3 0_3×3 0_3×3 0_3×3]

S106, while performing step S104, it is judged that the pedestrian level variation characteristic under by environmental constraint, real Time more new high degree observed quantity output it to Kalman filter and carry out height correction.

Fig. 3 is the navigation calculation flow chart highly constrained based on stairway step in environment.Utilize stair to top bar highly to enter During row constraint, need to first extract the zero-speed in gait interval, height change between adjacent point of zero velocity in then judging zero-speed interval Relation between value and setting height threshold value, identifies the kinestate (upstairs, downstairs, horizontal movement) of pedestrian.Definition k moment SINS The elevation information resolved is h_INS, the k-1 moment update after elevation information be h_prev, the difference in height of the two is designated as Δ h=h_INS- h_prev, height threshold is designated as th_h, the k moment update after elevation information be h_cuur, the now height value of navigation calculation and observation Relation between height value can be expressed as:

Δ H=h_INS-h_cuur

The measurement matrix of height correction is:

H_height=[0_3×3 0_3×3 [0 0 1] 0_3×3 0_3×3]

Perform can obtain revised pedestrian navigation parameter in real time by the circulation of step S102 to S106.

It should be appreciated that the application of the present invention is not limited to above-mentioned citing, for those of ordinary skills, can To be improved according to the above description or to convert, all these modifications and variations all should belong to the guarantor of claims of the present invention Protect scope.

Claims (4)

1. a pedestrian navigation system three-dimensional fix method based on human body/environmental constraints, it is characterised in that the method bag Include following steps:

S101, installs MEMS Inertial Measurement Unit, installs pressure transducer at sole heel at heel position；

S102, gathers MEMS Inertial Measurement Unit and pressure transducer output data, extracts the zero-speed district in pedestrian movement's gait Between；The formula in the concrete zero-speed interval extracted in pedestrian movement's gait is:

a_min＜ a_x(k)²+a_y(k)²+a_z(k)²＜ a_max

f(k)-T_f＞ 0

|ω_y(k) | ＜ T_ω

If above four Rule of judgment are set up simultaneously, then it is assumed that current time pedestrian foot remains static；

Wherein, a_iK () represents that k moment i axially goes up accelerometer output, i=x, y, z.a_min、a_maxIt is a left side for threshold interval respectively Border and right margin；a_kIt is accelerometer output,Being the meansigma methods of data point in window, n represents window width,For Variance threshold values；The force value that f (k) the expression k moment detects, T_fRepresent pressure threshold；| ω_y(k) | represent that k moment y-axis gyro is defeated The absolute value gone out, T_ωFor angular speed threshold value；

S103, utilizes acceleration, angular velocity information that in step S102, MEMS Inertial Measurement Unit is measured, and combines below equation Carry out strap-down navigation resolving, obtain the position of pedestrian, speed, attitude information；

Wherein, subscript n represents that navigational coordinate system, b represent carrier coordinate system.Q represents that attitude quaternion, V represent that speed, P represent Position.It is the transmission matrix being tied to n system by b,× represent the antisymmetric matrix that the angular velocity measured by gyroscope is constituted, f^bRepresent specific force,Represent rotational-angular velocity of the earth,Represent that navigation is the angle of rotation speed of relative earth system, gⁿRepresent ground Gravity field vector, can be approximately constant value in the most a certain scope；

S104, utilizes the zero-speed interval extracted in step S102 to carry out Kalman filtering, estimates pedestrian system state error also SINS calculation result is modified；

System Kalman filter model is:

δx_k+1=Φ_kδx_k+w_k

δz_k=H_kδx_k+v_k

Wherein, δ x_kIt is the system mode in k moment, Φ_kIt is state-transition matrix, w_kRepresent process noise, δ z_kSee for k moment error Measure, H_kFor calculation matrix, v_kFor measuring noise；

Definition error state vector is:

δ x=[δ φⁿ δVⁿ δPⁿ ε^b ▽^b]

Wherein comprising 9 navigation errors and 6 sensor errors in error state vector, they are three-dimensional site error respectively Vector δ φⁿ, velocity error vector δ Vⁿ, attitude vectors δ Pⁿ, gyro biased error vector ε^bAnd add table biased error vector^b；

State-transition matrix is:

Wherein, Δ t represents sampling time interval, fⁿ× under navigational coordinate system than force component constitute antisymmetric matrix；

In zero-speed interval, data noise expression formula is:

Δ V=V_INS-V_Stance

The measurement matrix of zero-velocity curve is:

H_zupt=[0_3×3 I_3×3 0_3×3 0_3×3 0_3×3]

S105, while performing step S104, utilizes the output of Magnetic Sensor to calculate each point of zero velocity pair in zero-speed interval The course angle answered, in step S104 goes to zero-speed interval during last point of zero velocity, to Magnetic Sensor meter in zero-speed interval The all course angles calculated make smothing filtering, and result are exported as observed quantity and carry out course to Kalman filter and repair Just；

S106, while performing step S104, it is judged that the height change characteristic of pedestrian under environmental constraint, real-time update is high Degree observed quantity also outputs it to Kalman filter and carries out height correction；

Perform can obtain revised pedestrian navigation parameter in real time by the circulation of step S102 to S106.

2. the method for claim 1, it is characterised in that above-mentioned MEMS Inertial Measurement Unit, comprises 3 axis MEMS and accelerates Degree meter, three axis MEMS gyro and 3 axis MEMS Magnetic Sensor.

3. the method for claim 1, it is characterised in that:

In described step S105: while performing step S104, each in utilizing the output of Magnetic Sensor to calculate zero-speed interval The course angle that individual point of zero velocity is corresponding, in step S104 goes to zero-speed interval during last point of zero velocity, in zero-speed interval All course angles that Magnetic Sensor calculates make smothing filtering, and export result to Kalman filter；In zero-speed interval The calculation expression of course angle and course angle measurement error is:

Heading=(Heading_total+Heading_Mag(n))/n

Wherein, n represents the number of point of zero velocity, Heading in zero-speed interval_totalLast point of zero velocity is removed in representing zero-speed interval The magnetic heading angle summation that other point of zero velocity outer is corresponding, Heading_MagI magnetic that in () expression zero-speed interval, i-th point of zero velocity is corresponding Course angle, in Heading zero-speed interval, all course angles of Magnetic Sensor calculating make the value of smothing filtering,Represent course angle Error,Represent the calculated course angle of SINS.

The measurement matrix of navigational calibration is:

H_heading=[[0 0 1] 0_3×3 0_3×3 0_3×3 0_3×3]。

4. the method for claim 1, it is characterised in that:

In described step S106: while performing step S104, it is judged that the pedestrian level change under by environmental constraint Characteristic, real-time update height observed quantity also outputs it to Kalman filter；Stair are utilized to top bar when highly retraining, Need to first extract the zero-speed in gait interval, in then judging zero-speed interval, between adjacent point of zero velocity, height change value is high with setting Relation between degree threshold value, identifies the kinestate of pedestrian；The elevation information that definition k moment SINS resolves is h_INS, the k-1 moment is more Elevation information after Xin is h_prev, the difference in height of the two is designated as Δ h=h_INS-h_prev, height threshold is designated as th_h, after the k moment updates Elevation information be h_cuur, now the relation between the height value of navigation calculation and the height value of observation can be expressed as:

Δ H=h_INS-h_cuur

The measurement matrix of height correction is:

H_height=[0_3×3 0_3×3 [0 0 1] 0_3×3 0_3×3]。