CN107289935B - Indoor navigation algorithm suitable for wearable equipment - Google Patents

Abstract

The invention provides an indoor navigation algorithm suitable for wearable equipment, which adopts a method of simultaneously constructing a WI-Fi fingerprint database and navigating and realizes indoor seamless navigation capability based on sensors such as an inertial sensitive component, a barometer and a magnetometer. The navigation algorithm integrates inertial sensor data, WI-Fi wireless network card data, barometer data and magnetometer data in a data fusion mode, and performs navigation calculation so as to output a navigation result. Meanwhile, a fingerprint database is generated in real time through nearby WI-Fi signal information collected by the wireless network card so as to assist navigation. The invention is an indoor navigation algorithm based on inertial devices and WI-Fi fingerprint identification, is based on low-cost MEMS inertial sensors, barometers, magnetometers and other devices, assists with a WI-Fi positioning algorithm, can rapidly generate and update a WI-Fi fingerprint database to assist in dead reckoning, further greatly improves indoor navigation capacity, is applicable to wearable equipment, and can effectively complete indoor navigation positioning.

Description

Indoor navigation algorithm suitable for wearable equipment

Technical Field

The invention belongs to the integrated navigation technology, and particularly relates to an indoor navigation positioning algorithm for assisting navigation by simultaneously constructing a WI-Fi (wireless fidelity) fingerprint database based on inertial device navigation.

Background

In an indoor environment, due to the great attenuation of satellite signals, a method based on satellite positioning cannot be implemented, and the problem of navigation positioning of the last kilometer is solved. Due to the fact that the demand of people for wireless internet surfing is increasing in recent years, more and more WI-Fi wireless access points are erected in buildings to meet the demand of people for seamless internet access, and further space is provided for indoor navigation technology based on WI-Fi signals. However, the traditional positioning technology based on Wi-FI position fingerprint information has large error and is easy to be interfered, and the efficiency of establishing and updating the fingerprint database is low. Meanwhile, the performance of the fingerprint database is greatly influenced by the movement or increase of the Wi-FI access points, so that the navigation effect is influenced.

Disclosure of Invention

The technical problem to be solved by the invention is as follows: an indoor navigation solution suitable for a wearable device is provided.

The technical scheme of the invention is as follows: the solution idea mainly comprises three steps: (1) indoor navigation is realized through a multi-sensor fusion technology taking an inertial sensor as a main technology, a pace detection technology, a zero correction technology and the like; (2) simultaneously constructing and updating a WI-Fi fingerprint database in real time; (3) navigation is assisted through a positioning result of WI-Fi fingerprint identification, and navigation precision is improved. The device used by the scheme can be packaged into a portable module, and the use requirement of wearable equipment is met. An indoor navigation algorithm suitable for wearable equipment comprises the following specific scheme: the method comprises the following steps:

an indoor navigation algorithm suitable for a wearable device, characterized by the steps of:

step 1: initialization phase

In the product initialization stage, an indoor map needs to be imported and an initial position needs to be designated, and a user selects a current indoor map based on an indoor map set built in advance and manually designates the current position; if the map is not available, the map can be downloaded through a network database;

step 2: multi-sensor based dead reckoning

a) Preprocessing gyroscope and counting data

Marking the three-axis gyro signals as NX, NY and NZ respectively, performing low-pass filtering on the three-axis gyro signals to filter out external high-frequency noise,

GX＝B0·NX+B1·NX_1+B2·NX_2+B3·NX_3+B4·NX_4+B5·NX_5+...B6·NX_6+B7·NX_7-(A1·GX_1+A2·GX_2+A3·GX_3+A4·GX_4+...A5·GX_5+A6·GX_6+A7·GX_7)

GY＝B0·NY+B1·NY_1+B2·NY_2+B3·NY_3+B4·NY_4+B5·NY_5+...B6·NY_6+B7·NY_7-(A1·GY_1+A2·GY_2+A3·GY_3+A4·GY_4+...A5·GY_5+A6·GY_6+A7·GY_7)

GZ＝B0·NZ+B1·NZ_1+B2·NZ_2+B3·NZ_3+B4·NZ_4+B5·NZ_5+...B6·NZ_6+B7·NZ_7-(A1·GZ_1+A2·GZ_2+A3·GZ_3+A4·GZ_4+...A5·GZ_5+A6·GZ_6+A7·GZ_7)

wherein, the data collected at each sampling moment is NX, the output value after filtering is GX,

NX _1 is a last sampling value, NX _2 is a last sampling value, and the like;

GX _1 is the last filtered value, GX _2 is the last filtered value, and the other steps are similar;

A1-A7 and B0-B7 are filter parameters which need to be properly selected according to a gyroscope addition signal;

the accelerometer obtains gyroscope outputs GX, GY and GZ and accelerometer outputs AX, AY and AZ in the same way;

b) magnetometer determines heading

Only positioning is carried out when the device is static, the magnetic heading is taken as the initial heading at the initial moment when the device is moved, and the initial alignment is not carried out;

heading angle psi ═ magnetometer heading angle

Pitch angle θ is 0

Roll angle γ is 0

c) Strapdown inertial solution

Computing

C₁₁＝cos(γ)·cos(ψ)+sin(γ)·sin(θ)·sin(ψ)

C₁₂＝cos(θ)·sin(ψ)

C₁₃＝sin(γ)·cos(ψ)-cos(γ)·sin(θ)·sin(ψ)

C₂₁＝-cos(γ)·sin(ψ)+sin(γ)·sin(θ)·cos(ψ)

C₂₂＝cos(θ)·cos(ψ)

C₂₃＝-sin(γ)·sin(ψ)-cos(γ)·sin(θ)·cos(ψ)

C₃₁＝-cosθ·sinγ

C₃₂＝sinθ

C₃₃＝cosθ·cosγ

Computing

Is the angle increment, delta phi, measured by an X-axis inertial device under the b system_xIs the zero error of the X-axis gyroscope,

is the angular increment of the x-axis gyroscope b relative to the n system;

the definition is similar.

d) Attitude angle zero speed correction using gyro data

Platform declination correction

φ_U、φ_N、φ_E、

Respectively, the platform deflection angles in the sky direction, the north direction and the east direction, and the attitude matrix;

e) zero-speed correction of position by using count-up data

L＝L(-)-δL

λ＝λ(-)-δλ

L(-)＝L

λ(-)＝λ

L and lambda are respectively latitude and longitude;

note: the process is a cycle, the longitude and latitude calculated by the first beat is recorded as lambda (-), L (-), the longitude and latitude calculated by the second beat is recorded as lambda (-), L (-), and the like when the second beat is calculated;

speed correction

V_E＝V_E(-)-δV_E

V_N＝V_N(-)-δV_N

V_E(-)＝V_E

V_N(-)＝V_N

V_E、V_NEast speed and north speed respectively;

note: here, the loop is executed, and in the second beat calculation, the northeast speed calculated in the first beat is denoted as V_E(-)、V_N(-) when the third beat is calculated, the longitude and latitude calculated by the second beat is recorded as V_E(-)、V_N(-), and so on;

f) damping height of barometer

V_U＝V_U(-)+K₁(h_b-h)

V_U(-)＝V_U

V_U、h_bH is the speed in the direction of the day, the height of the organism system and the actual height respectively;

note: here, the loop is performed, and when the second beat is calculated, the daily speed calculated in the first beat is recorded as V_U(-) when the third beat is calculated, the longitude and latitude calculated by the second beat is recorded as V_U(-), and so on;

wherein: k₁＝0.0005

If the height damping calculation condition is met, damping calculation is carried out according to the following formula:

h＝h(-)+K₂·(h_b-h(-))

h(-)＝h

note: the circulation is performed, when the second beat is calculated, the actual height calculated by the first beat is recorded as h (-), when the third beat is calculated, the longitude and latitude calculated by the second beat is recorded as h (-), and so on;

wherein: k₂＝0.01

Judging the pace speed and carrying out basic track calculation based on the six steps;

and step 3: WI-Fi fingerprint database generation and update

In this stage, the WI-Fi network card receives Beacon signals (Beacon Signal) from nearby wireless Access Points (APs) in real time, wherein the Beacon signals contain MAC address information and Signal strength (RSSI) information of each AP; the WI-Fi fingerprint database contains a set of data intensity values which can receive beacon signals sent by nearby APs at any point in the indoor environment;

assuming that each location can be represented by an RSSI vector from a different wireless access point, then a location a can be represented as:

the vector of RSS values representing the location, and the RSS₁To RSS_nRepresenting the strength of signals received from different wireless access points at random location a; the position information and the signal strength information of each sampling point are contained in a complete Wi-Fi fingerprint database; thus, the Wi-Fi fingerprint database for a region can be written as:

wherein x_iAnd y_iRespectively representing the coordinate positions of the sampling points, wherein m represents the number of the sampling points; in the scheme, a mode of quickly generating and updating the database in real time is used, namely a machine learning method based on a Gaussian process is adopted; assume that each sample point can be written as:

Loca＝{(x_i,y_i)|i＝1,2,...,n}

where x represents the input vector (i.e., coordinates) and y represents the measurement; assume that the data is taken from a noisy process:

y_i＝f(x_i)+ε

where the error ε is obeyed to mean zero and the variance is

(ii) a gaussian distribution of; by means of the optimized covariance function (kernel) k (x)_i，x_j) We can derive f (x)_i) And f (x)_j) The relationship between; using the covariance function, f (x)_i) Function value representing the xi point, similarly, f (x)_j) Function value representing the xj point:

realizing Gaussian process regression; by the method, the database under the whole environment can be quickly generated through limited sampling points; the refreshing time is 1 second/time, and the WI-Fi fingerprint database is updated once every refreshing;

and 4, step 4: multi-information source fusion navigation algorithm

a) According to the wifi signal comparison data fused in the step 3, the indoor position is defined, inertial navigation damping barometer data and wifi signals are fused, and the floor is defined

Δφ_x＝(C₁₁·ω_e'+C₂₁·ω_n'+C₃₁·ω_u')·Ts

Δφ_y＝(C₁₂·ω_e'+C₂₂·ω_n'+C₃₂·ω_u')·Ts

Δφ_z＝(C₁₃·ω_e'+C₂₃·ω_n'+C₃₃·ω_u')·Ts

Wherein:

du₁＝(ω₈-ω₇)×(V_N-K₁·V_rN)-ω₇·u₁

du₂＝(ω₅-ω₆)×(V_N-K₁·V_rN+u₁)-ω₆·u₂

du₃＝(ω₄-ω₃)×(V_E-K₁·V_rE)-ω₃·u₃

du₄＝(ω₁-ω₂)×(V_E-K₁·V_rE+u₃)-ω₂·u₄

du₅＝C×B×(V_N-K₁·V_rN+u₁+u₂)-ω₉·u₅

du₆＝ω₁₀·u₅-ω₁₀·u₆

u₁＝u₁(-)+du₁×T_S

u₂＝u₂(-)+du₂×T_S

u₃＝u₃(-)+du₃×T_S

u₄＝u₄(-)+du₄×T_S

u₅＝u₅(-)+du₅×T_S

u₆＝u₆(-)+du₆×T_S

u₁(-)＝u₁

u₂(-)＝u₂

u₃(-)＝u₃

u₄(-)＝u₄

u₅(-)＝u₅

u₆(-)＝u₆

note: the process is a cycle, when the second beat is calculated, the actual height calculated by the first beat is recorded as u (-), when the third beat is calculated, the longitude and latitude calculated by the second beat is recorded as u (-), and so on;

D＝sinL

E＝-cosL

R_E＝R·[1+e·sin²(L)]

R_N＝R·[1-2e+3e·sin²(L)]

u₁、u₂、u₃、u₄、u₅、u₆initial values are all 0

R、R_E、R_NThe average radius of the earth, the radius of the earth's major semi-axis and the radius of the earth's minor semi-axis

V_rE、V_rNEast and north speeds resolved in real time for measuring speed (e.g. odometer, etc.), respectively

Ts is the sampling time, ω_e'，ω_n'，ω_u' is the east, north, and sky angular velocity of the damping loop; u. of₁,u₂,u₃,u₄,u₅,u₆The method is a formula intermediate variable without dimension, and can be calculated by giving an initial value and an increment;

ω₁、ω₂、ω₃、ω₄、ω₅、ω₆、ω₇、ω₈、ω₉、ω₁₀、A，B，C，D,E,K₁selecting according to an actual system for calculating coefficients without dimension;

b) design of extended Kalman algorithm to realize indoor navigation positioning

δ L and δ λ are latitude error and longitude error, respectively;

δV_E、δV_Neast and north velocity errors, respectively;

φ_E、φ_N、φ_Urespectively an east platform deflection angle, a north platform deflection angle and a sky platform deflection angle;

δD_X、δD_Y、δD_Z、

respectively representing angular velocity increment in the X-axis direction, angular velocity increment in the Y-axis direction, angular velocity increment in the Z-axis direction, velocity increment in the X-axis direction, velocity increment in the Y-axis direction and velocity increment in the Z-axis direction;

δα₁、δα₂、δα₃、δα₄δ ε represents the 5 declination angles between the inertial devices measuring the angular and velocity increments, respectively;

δK_AY、δK_AZscale coefficient errors of the inertial device in the Y direction and the Z direction respectively representing the measured acceleration;

X_K/K-1＝φ_K/K-1·X_K-1

P_K-1＝P_K/K-1

X_K-1＝X_K/K-1

X_K＝X_K/K-1+K_K·(Z_K-H_K·X_K/K-1)

P_K＝(I-K_K·H_K)·P_K/K-1

P_K-1＝P_K

φ_K/K-1representing a one-step estimate of phi in the Kalman Filter estimation, T_FFor filtering time, Q_K-1As noise at the last moment, X_K/K-1For one-step prediction of X, P_K/K-1For one-step prediction of P, X_KIs an estimate of time k, P_KTaking the noise value at the moment K, F as a matrix and P as a designed initial value of a noise error matrix; wherein I is a unit array; z_KMeasured at time k, H_KFor the measurement array, Q is the system noise variance array, K_KIs the filter gain;

represents H_KThe transpose of (a) is performed,

represents phi_K/K-1The transposing of (1).

When the zero speed correction condition is satisfied:

δL＝X′_K (1)

δλ＝X′_K (2)

X′_K(1) is X_KTransposed first data, X'_K(2) Is X_KTransposed second data;

is X_KThe 21 st data averaged over one period,

is X_KData number 22 averaged over one period;

otherwise:

δL＝0

δλ＝0

δD_N、δD_Ukeeping the same;

measurement matrix

Wherein: f. of_E、f_N、f_URespectively representing the specific forces of the east direction, the north direction and the sky direction, namely the calculated value of the acceleration,

respectively represents the average value of the specific forces of the X axis, the Y axis and the Z axis of the linear system in one period, namely the average value of the components of the acceleration in three directions of the linear system, omega_N、ω_URespectively representing the angular velocities in the north direction and the sky direction, namely the calculated angular velocity,

respectively representing the average of the angular velocities of the X-axis, Y-axis and Z-axis of the machine system in one cycle, i.e. the angular velocity is a component of the machine system in three directionsMean value of

Measurement matrix during zero-speed calibration:

P＝diag(6e-4，6e-4，1，1，6e-6，6e-6，3.0e-4，1e-12，1e-12，1e-12，2.5e-5，2.5e-5，2.5e-5，0，0，0，0，0，0，0,0,0)

Q＝diag(0，0，5.e-5，5.e-5，5.e-13，5.e-13，5.e-13，0，0，0，0，0，0,0,0,0,0,0，0，0,0,0)R＝diag(2.6e-10，2.6e-10)

when the wifi access point is replaced in the dead reckoning process, P is required to be adjusted_1,1、P_2,2Resetting is carried out:

P_1,1＝6e-4

P_2,2＝6e-4

zero-speed correction of the filter parameters

Q＝diag(0，0，0，0，5.e-13，5.e-13，5.e-13，0，0，0，0，0，0,0,0,0,0,0，0，0，0，0)

R＝diag(2.6e-10，2.6e-10，9，9)

And 5: navigation result display

The navigation result is transmitted to the terminal display through the wireless network to be displayed, so that the task of indoor navigation is completed.

And performing inertial navigation according to the multi-sensitive component, and performing real-time comparison and correction by using wifi signals to realize indoor navigation.

The invention has the beneficial effects that: the indoor navigation algorithm based on the inertial device and the WI-Fi fingerprint identification is based on low-cost MEMS inertial sensors, barometers, magnetometers and other devices, and is assisted by a WI-Fi positioning algorithm, so that the WI-Fi fingerprint database can be rapidly generated and updated to assist in dead reckoning, and further indoor navigation capacity is greatly improved. This scheme is applicable to on the wearable equipment, can effectual completion indoor navigation location's work.

Drawings

FIG. 1 is a process flow diagram of the present invention.

Detailed Description

As shown in fig. 1, the technical solution of the present invention is: an indoor navigation algorithm suitable for a wearable device is characterized by comprising the following steps:

1.1) initializing map information and location information

1.2) inertial navigation algorithm based on MEMS gyroscope

1.3) analyzing the gyro accelerometer signal, finding out the calculation point of the pace speed, and carrying out the pace speed calculation

1.4) zero correction by speed when stationary

1.5) real-time correction in the presence of GPS signals

1.6) generating and updating the WI-Fi fingerprint database in real time by using a machine learning algorithm based on Gaussian process regression

1.7) WI-Fi fingerprinting technology positioning

1.8) adopting an extended Kalman filter to perform fusion calculation on the information source so as to calculate a navigation result

1.9) navigation result display

The result shows that the indoor navigation accuracy with the deviation of 1 meter (RMS) can be realized by adopting the method based on the indoor navigation algorithm of the wearable device.

Those skilled in the art will appreciate that the invention may be practiced without these specific details.

Finally, it should be noted that: the above embodiments are merely illustrative and not restrictive of the technical solutions of the present invention, and all modifications or partial replacements that do not depart from the spirit and scope of the present invention should be embraced in the claims of the present invention.

Claims (2)

1. An indoor navigation algorithm suitable for a wearable device, characterized by the steps of:

step 1: initialization phase

In the product initialization stage, an indoor map needs to be imported and an initial position needs to be designated, and a user selects a current indoor map based on an indoor map set built in advance and manually designates the current position; if the map is not available, the map can be downloaded through a network database;

step 2: multi-sensor based dead reckoning

a) Preprocessing gyroscope and counting data

Marking the three-axis gyro signals as NX, NY and NZ respectively, performing low-pass filtering on the three-axis gyro signals to filter out external high-frequency noise,

GX＝B0·NX+B1·NX_1+B2·NX_2+B3·NX_3+B4·NX_4+B5·NX_5+...B6·NX_6+B7·NX_7-(A1·GX_1+A2·GX_2+A3·GX_3+A4·GX_4+...A5·GX_5+A6·GX_6+A7·GX_7)

GY＝B0·NY+B1·NY_1+B2·NY_2+B3·NY_3+B4·NY_4+B5·NY_5+...B6·NY_6+B7·NY_7-(A1·GY_1+A2·GY_2+A3·GY_3+A4·GY_4+...A5·GY_5+A6·GY_6+A7·GY_7)

GZ＝B0·NZ+B1·NZ_1+B2·NZ_2+B3·NZ_3+B4·NZ_4+B5·NZ_5+...B6·NZ_6+B7·NZ_7-(A1·GZ_1+A2·GZ_2+A3·GZ_3+A4·GZ_4+...A5·GZ_5+A6·GZ_6+A7·GZ_7)

wherein, the data collected at each sampling moment is NX, the output value after filtering is GX,

NX _1 is a last sampling value, NX _2 is a last sampling value, and the like;

GX _1 is the last filtered value, GX _2 is the last filtered value, and the other steps are similar;

A1-A7 and B0-B7 are filter parameters which need to be properly selected according to a gyroscope addition signal;

the accelerometer obtains gyroscope outputs GX, GY and GZ and accelerometer outputs AX, AY and AZ in the same way;

b) magnetometer determines heading

Only positioning is carried out when the device is static, the magnetic heading is taken as the initial heading at the initial moment when the device is moved, and the initial alignment is not carried out;

heading angle psi ═ magnetometer heading angle

Pitch angle θ is 0

Roll angle γ is 0

c) Strapdown inertial solution

Computing

C₁₁＝cos(γ)·cos(ψ)+sin(γ)·sin(θ)·sin(ψ)

C₁₂＝cos(θ)·sin(ψ)

C₁₃＝sin(γ)·cos(ψ)-cos(γ)·sin(θ)·sin(ψ)

C₂₁＝-cos(γ)·sin(ψ)+sin(γ)·sin(θ)·cos(ψ)

C₂₂＝cos(θ)·cos(ψ)

C₂₃＝-sin(γ)·sin(ψ)-cos(γ)·sin(θ)·cos(ψ)

C₃₁＝-cosθ·sinγ

C₃₂＝sinθ

C₃₃＝cosθ·cosγ

Computing

Is the angle increment, delta phi, measured by an X-axis inertial device under the b system_xIs the zero error of the X-axis gyroscope,

is the angular increment of the x-axis gyroscope b relative to the n system;

the definitions are similar;

d) attitude angle zero speed correction using gyro data

Platform declination correction

φ_U、φ_N、φ_E、

Respectively, the platform deflection angles in the sky direction, the north direction and the east direction, and the attitude matrix;

e) zero-speed correction of position by using count-up data

L＝L(-)-δL

λ＝λ(-)-δλ

L(-)＝L

λ(-)＝λ

L and lambda are respectively latitude and longitude;

note: the process is a cycle, the longitude and latitude calculated by the first beat is recorded as lambda (-), L (-), the longitude and latitude calculated by the second beat is recorded as lambda (-), L (-), and the like when the second beat is calculated;

speed correction

V_E＝V_E(-)-δV_E

V_N＝V_N(-)-δV_N

V_E(-)＝V_E

V_N(-)＝V_N

V_E、V_NEast speed and north speed respectively;

note: here, the loop is executed, and in the second beat calculation, the northeast speed calculated in the first beat is denoted as V_E(-)、V_N(-) when the third beat is calculated, the longitude and latitude calculated by the second beat is recorded as V_E(-)、V_N(-), and so on;

f) damping height of barometer

V_U＝V_U(-)+K₁(h_b-h)

V_U(-)＝V_U

V_U、h_bH is the speed in the direction of the day, the height of the organism system and the actual height respectively;

note: here, the loop is performed, and when the second beat is calculated, the daily speed calculated in the first beat is recorded as V_U(-) when the third beat is calculated, the longitude and latitude calculated by the second beat is recorded as V_U(-), and so on;

wherein: k₁＝0.0005

If the height damping calculation condition is met, damping calculation is carried out according to the following formula:

h＝h(-)+K₂·(h_b-h(-))

h(-)＝h

note: the circulation is performed, when the second beat is calculated, the actual height calculated by the first beat is recorded as h (-), when the third beat is calculated, the longitude and latitude calculated by the second beat is recorded as h (-), and so on;

wherein: k₂＝0.01

Judging the pace speed and carrying out basic track calculation based on the six steps;

and step 3: WI-Fi fingerprint database generation and update

In this stage, the WI-Fi wireless network card receives Beacon signals (Beacon Signal) from nearby wireless Access Points (APs) in real time, wherein the Beacon signals contain MAC address information and Signal strength RSSI information of each AP; the WI-Fi fingerprint database contains a set of data intensity values which can receive beacon signals sent by nearby APs at any point in the indoor environment;

assuming that each location can be represented by an RSSI vector from a different wireless access point, then a location a can be represented as:

the vector of RSS values representing the location, and the RSS₁To RSS_nRepresenting the strength of signals received from different wireless access points at random location a; the position information and the signal strength information of each sampling point are contained in a complete Wi-Fi fingerprint database; thus, the Wi-Fi fingerprint database for a region can be written as:

wherein x_iAnd y_iRespectively representing the coordinate positions of the sampling points, wherein m represents the number of the sampling points; in the scheme, a mode of quickly generating and updating the database in real time is used, namely a machine learning method based on a Gaussian process is adopted; assume that each sample point can be written as:

Loca＝{(x_i，y_i)|i＝1，2，...，n}

where x represents the input vector (i.e., coordinates) and y represents the measurement; assume that the data is taken from a noisy process:

y_i＝f(x_i)+ε

where the error ε is obeyed to mean zero and the variance is

Gaussian distribution of(ii) a By means of the optimized covariance function (kernel) k (x)_i，x_j) We can derive f (x)_i) And f (x)_j) The relationship between; using the covariance function, f (x)_i) Function value representing the xi point, similarly, f (x)_j) Function value representing the xj point:

realizing Gaussian process regression; by the method, the database under the whole environment can be quickly generated through limited sampling points; the refreshing time is 1 second/time, and the WI-Fi fingerprint database is updated once every refreshing;

and 4, step 4: multi-information source fusion navigation algorithm

a) According to the wifi signal comparison data fused in the step 3, the indoor position is defined, inertial navigation damping barometer data and wifi signals are fused, and the floor is defined

Δφ_x＝(C₁₁·ω_e′+C₂₁·ω_n′+C₃₁·ω_u′)·Ts

Δφ_y＝(C₁₂·ω_e'+C₂₂·ω_n'+C₃₂·ω_u')·Ts

Δφ_z＝(C₁₃·ω_e'+C₂₃·ω_n'+C₃₃·ω_u')·Ts

Wherein:

du₁＝(ω₈-ω₇)×(V_N-K₁·V_rN)-ω₇·u₁

du₂＝(ω₅-ω₆)×(V_N-K₁·V_rN+u₁)-ω₆·u₂

du₃＝(ω₄-ω₃)×(V_E-K₁·V_rE)-ω₃·u₃

du₄＝(ω₁-ω₂)×(V_E-K₁·V_rE+u₃)-ω₂·u₄

du₅＝C×B×(V_N-K₁·V_rN+u₁+u₂)-ω₉·u₅

du₆＝ω₁₀·u₅-ω₁₀·u₆

u₁＝u₁(-)+du₁×T_S

u₂＝u₂(-)+du₂×T_S

u₃＝u₃(-)+du₃×T_S

u₄＝u₄(-)+du₄×T_S

u₅＝u₅(-)+du₅×T_S

u₆＝u₆(-)+du₆×T_S

u₁(-)＝u₁

u₂(-)＝u₂

u₃(-)＝u₃

u₄(-)＝u₄

u₅(-)＝u₅

u₆(-)＝u₆

note: the process is a cycle, when the second beat is calculated, the actual height calculated by the first beat is recorded as u (-), when the third beat is calculated, the longitude and latitude calculated by the second beat is recorded as u (-), and so on;

D＝sin L

E＝-cos L

R_E＝R·[1+e·sin²(L)]

R_N＝R·[1-2e+3e·sin²(L)]

u₁、u₂、u₃、u₄、u₅、u₆initial values are all 0

R、R_E、R_NThe average radius of the earth, the radius of the earth's major semi-axis and the radius of the earth's minor semi-axis

V_rE、V_rNEast and north speeds resolved in real time for measuring speed (e.g. odometer, etc.), respectively

Ts is the sampling time, ω_e'，ω_n'，ω_u' is the east, north, and sky angular velocity of the damping loop;

u₁,u₂,u₃,u₄,u₅,u₆the method is a formula intermediate variable without dimension, and can be calculated by giving an initial value and an increment;

ω₁、ω₂、ω₃、ω₄、ω₅、ω₆、ω₇、ω₈、ω₉、ω₁₀、A，B，C，D,E,K₁selecting according to an actual system for calculating coefficients without dimension;

b) design of extended Kalman algorithm to realize indoor navigation positioning

δ L and δ λ are latitude error and longitude error, respectively;

δV_E、δV_Neast and north velocity errors, respectively;

φ_E、φ_N、φ_Urespectively an east platform deflection angle, a north platform deflection angle and a sky platform deflection angle;

δD_X、δD_Y、δD_Z、

respectively representing angular velocity increment in the X-axis direction, angular velocity increment in the Y-axis direction, angular velocity increment in the Z-axis direction, velocity increment in the X-axis direction, velocity increment in the Y-axis direction and velocity increment in the Z-axis direction;

δα₁、δα₂、δα₃、δα₄δ ε represents the 5 declination angles between the inertial devices measuring the angular and velocity increments, respectively;

δK_AY、δK_AZscale coefficient errors of the inertial device in the Y direction and the Z direction respectively representing the measured acceleration;

X_K/K-1＝φ_K/K-1·X_K-1

P_K-1＝P_K/K-1

X_K-1＝X_K/K-1

X_K＝X_K/K-1+K_K·(Z_K-H_K·X_K/K-1)

P_K＝(I-K_K·H_K)·P_K/K-1

P_K-1＝P_K

φ_K/K-1representing a one-step estimate of phi in the Kalman Filter estimate, T_FFor filtering time, Q_K-1As noise at the last moment, X_K/K-1For one-step prediction of X, P_K/K-1For one-step prediction of P, X_KIs an estimate of time k, P_KTaking the noise value at the moment K, F as a matrix and P as a designed initial value of a noise error matrix; wherein I is a unit array; z_KMeasured at time k, H_KFor the measurement array, Q is the system noise variance array, K_KIs the filter gain;

represents H_KThe transpose of (a) is performed,

represents phi_K/K-1Transposing;

when the zero speed correction condition is satisfied:

δL＝X′_K (1)

δλ＝X′_K (2)

X′_K(1) is X_KTransposed first data, X'_K(2) Is X_KTransposed second data;

(21) is X_KThe 21 st data averaged over one period,

(22) is X_KData number 22 averaged over one period;

otherwise:

δL＝0

δλ＝0

δD_N、δD_Ukeeping the same;

measurement matrix

Wherein: f. of_E、f_N、f_URespectively representing the specific forces of the east direction, the north direction and the sky direction, namely the calculated value of the acceleration,

respectively represents the average value of the specific forces of the X axis, the Y axis and the Z axis of the linear system in one period, namely the average value of the components of the acceleration in three directions of the linear system, omega_N、ω_URespectively representing the angular velocities in the north direction and the sky direction, namely the calculated angular velocity,

respectively represents the average value of the angular speeds of the X axis, the Y axis and the Z axis of the machine system in one period, namely the average value of the angular speeds of three directional components of the machine system

Measurement matrix during zero-speed calibration:

P＝diag(6e-4，6e-4，1，1，6e-6，6e-6，3.0e-4，1e-12，1e-12，1e-12，2.5e-5，2.5e-5，2.5e-5，0，0，0，0，0，0，0,0,0)

Q＝diag(0，0，5.e-5，5.e-5，5.e-13，5.e-13，5.e-13，0，0，0，0，0，0,0,0,0,0,0，0，0,0,0)

R＝diag(2.6e-10，2.6e-10)

when the wifi access point is replaced in the dead reckoning process, P is required to be adjusted_1,1、P_2,2Resetting is carried out:

P_1,1＝6e-4

P_2,2＝6e-4

zero-speed correction of the filter parameters

Q＝diag(0，0，0，0，5.e-13，5.e-13，5.e-13，0，0，0，0，0，0,0,0,0,0,0，0，0，0，0)

R＝diag(2.6e-10，2.6e-10，9，9)

And 5: navigation result display

The navigation result is transmitted to the terminal display through the wireless network to be displayed, so that the task of indoor navigation is completed.

2. The algorithm of claim 1, wherein: and performing inertial navigation according to the multi-sensitive component, and performing real-time comparison and correction by using wifi signals to realize indoor navigation.

Images