Pedestrian navigation adaptive Kalman filtering method based on frequency domain analysis

Abstract

The invention provides a pedestrian navigation adaptive Kalman filtering method based on frequency domain analysis, which comprises the following steps: binding a micro inertial sensor to the foot of a pedestrian, and collecting three-axis acceleration and three-axis angular velocity data of pedestrian movement; fourier transform is carried out on the three-axis acceleration data and the three-axis angular velocity data, and a frequency domain signal of each axis is obtained; according to the maximum amplitude value of each axis frequency domain signal in the triaxial acceleration and the triaxial angular velocity, a forgetting factor is constructed, and a weight coefficient is constructed based on the forgetting factor; adaptive estimation is carried out on the measurement noise covariance matrix in a weighting mode; and constructing a Kalman filter, and estimating a navigation error by using Kalman filtering. According to the method, the measurement noise is adaptively adjusted by using the frequency domain characteristics of the inertial data, the error estimation precision of the variable stride frequency motion is greatly improved, and high-precision positioning navigation of pedestrians is realized.

Description

Technical Field

[0001] The invention belongs to the technical field of pedestrian navigation, and in particular relates to a pedestrian navigation adaptive Kalman filtering method based on frequency domain analysis. Background Art

[0002] The foot-bound pedestrian navigation system connects the micro-inertial sensor to the human foot, detects the moment when the foot surface touches the ground and reaches zero speed, uses the speed error as the observed quantity, and uses Kalman filtering to suppress the divergence of navigation error.

[0003] In the traditional pedestrian navigation method based on zero-speed correction, the measurement noise covariance matrix remains unchanged when performing Kalman filtering, resulting in a large deviation in the zero-speed observation, which reduces the accuracy of error estimation and affects the navigation accuracy. Summary of the invention

[0004] In view of the technical problem in the prior art that the error estimation accuracy of the pedestrian navigation method based on zero-speed correction is reduced when the zero-speed observation value has a large deviation, the present invention provides a pedestrian navigation adaptive Kalman filtering method based on frequency domain analysis, which utilizes the frequency domain characteristics of inertial data to adaptively adjust the measurement noise, greatly improves the error estimation accuracy of variable step frequency motion, and realizes high-precision positioning and navigation of pedestrians.

[0005] The technical solution adopted by the present invention to solve the above technical problems is as follows:

[0006] The present invention proposes a pedestrian navigation adaptive Kalman filtering method based on frequency domain analysis, comprising the following steps:

[0007] Bind the micro-inertial sensor to the pedestrian's foot to collect the pedestrian's three-axis acceleration and three-axis angular velocity data;

[0008] Perform Fourier transform on the three-axis acceleration and three-axis angular velocity data to obtain the frequency domain signal of each axis;

[0009] According to the maximum amplitude value of the frequency domain signal of each axis in the three-axis acceleration and the three-axis angular velocity, a forgetting factor is constructed, and a weight coefficient is constructed based on the forgetting factor;

[0010] Adaptively estimate the measurement noise covariance matrix in a weighted manner;

[0011] Construct a Kalman filter and use it to estimate navigation errors.

[0012] Furthermore, the method for constructing the forgetting factor according to the maximum value of the frequency domain signal amplitude of each axis in the three-axis acceleration and the three-axis angular velocity is as follows:

[0013]

[0014] Among them, a gyr_x 、a gyr_y 、a gyr_z They are the maximum amplitude values ​​of the frequency domain signals of the angular velocity signals of the X, Y and Z axes respectively;

[0015] a acc_x 、a acc_y 、a acc_z They are the maximum amplitude values ​​of the frequency domain signals of the X, Y, and Z axis acceleration signals respectively;

[0016] α is the forgetting factor.

[0017] Furthermore, the weight coefficient is constructed based on the forgetting factor as follows:

[0018]

[0019] Among them, θ k ,θ k-1 are the weight coefficients at time k and time k-1 respectively.

[0020] Furthermore, the initial value of the weight coefficient is 1.

[0021] Furthermore, the method for adaptively estimating the measurement noise covariance matrix in a weighted manner is as follows:

[0022] Construct new information as:

[0023]

[0024] The adaptive estimation method of the measurement noise covariance matrix R is as follows:

[0025]

[0026] in, is the new information from time k-1 to time k; H k is the measurement matrix; I is the unit matrix; θ k is the weight coefficient at time k; P k,k-1 is the one-step prediction covariance matrix; are the adaptive estimates of the measurement noise covariance matrix at time k and time k-1 respectively.

[0027] Furthermore, the steps of constructing the Kalman filter are as follows:

[0028] The state equation and measurement equation of the Kalman filter system are:

[0029] X k =Φ k,k-1 X k-1 +Γ k-1 w k-1

[0030] Z k =H k X k +v k

[0031] Among them, X k is the system state vector at time k, X k-1 is the system state vector at time k-1;

[0032] Φ k,k-1 is the state transfer matrix;

[0033] w k-1 is the system noise;

[0034] Γ k-1 is the system noise input matrix;

[0035] Z k is the measurement vector;

[0036] H k is the measurement matrix;

[0037] v k To measure noise;

[0038] The measurement vector Z of the Kalman filter k is the speed error in the zero speed range:

[0039]

[0040] in, They represent the north speed, day speed and east speed at time k respectively.

[0041] Furthermore, the system noise w at time k is k and measurement noise v k Satisfies the following relationship:

[0042]

[0043] Among them, Q k represents the system noise covariance matrix, which is a non-negative definite matrix;

[0044] R k represents the measurement noise covariance matrix, which is a positive definite matrix;

[0045] E[] indicates the calculation of the mean;

[0046] Cov[] represents w k and v k The covariance of

[0047] δ kj is the Kronecker function, that is:

[0048]

[0049] j represents a time different from k.

[0050] Furthermore, the Kalman filter system state variables are:

[0051]

[0052] Among them, δV n , δV u , δV e Represent the velocity errors in the north, sky, and east directions respectively;

[0053] δL, δh, and δλ represent latitude error, altitude error, and longitude error, respectively;

[0054] φ n ,φ u ,φ e Respectively represent the misalignment angles in the north, sky and east directions;

[0055] Respectively represent the accelerometer zero bias in the X, Y, and Z directions;

[0056] ε x , ε y , ε z Respectively represent the gyro drift in the X, Y, and Z directions.

[0057] Furthermore, the Fourier transform method is as follows:

[0058]

[0059] Among them, f(t) is the acceleration or angular velocity signal of a certain axis, S(ω) is the transformed frequency domain signal, t represents time, and ω represents frequency.

[0060] The beneficial effects of the present invention compared with the prior art are as follows:

[0061] The present invention proposes an adaptive Kalman filter method for pedestrian navigation based on frequency domain analysis, which uses pedestrian motion characteristics to adaptively adjust filter parameters, uses Fourier transform to extract frequency domain features such as amplitude, constructs forgetting factors, and adaptively estimates the measurement noise covariance matrix in a weighted manner, further improving the error estimation and correction accuracy under actual motion conditions such as variable step frequency. The present invention greatly improves the accuracy of pedestrian navigation based on zero-speed correction. BRIEF DESCRIPTION OF THE DRAWINGS

[0062] The included drawings are used to provide a further understanding of the embodiments of the present invention, which constitute a part of the specification, are used to illustrate the embodiments of the present invention, and together with the text description, explain the principles of the present invention. Obviously, the drawings in the following description are only some embodiments of the present invention, and for ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0063] Figure 1 A flow chart of a pedestrian navigation adaptive Kalman filtering method based on frequency domain analysis provided in a specific embodiment of the present invention. DETAILED DESCRIPTION

[0064] Specific embodiments of the present invention are described in detail below. In the following description, for the purpose of explanation and not limitation, specific details are set forth to help fully understand the present invention. However, it will be apparent to those skilled in the art that the present invention may also be practiced in other embodiments that depart from these specific details.

[0065] It should be noted that in order to avoid obscuring the present invention due to unnecessary details, only the device structure and / or processing steps closely related to the scheme of the present invention are shown in the drawings, while other details that are not closely related to the present invention are omitted.

[0066] The present invention belongs to the field of pedestrian navigation technology based on micro-inertial systems, and relates to a pedestrian navigation adaptive Kalman filtering method based on frequency domain analysis. The foot-bound pedestrian navigation system connects the micro-inertial sensor to the human foot, detects the moment when the foot surface touches the ground and the speed error is zero, and uses Kalman filtering to suppress the divergence of navigation errors.

[0067] In order to use pedestrian motion characteristics to achieve more accurate estimation and correction of navigation errors, the present invention performs Fourier transform on the original inertial data to obtain the amplitude and other characteristics of the frequency domain data, and constructs the forgetting factor and weighting coefficient based on this to implement adaptive Kalman filtering and improve navigation accuracy. Specifically, the following steps are included:

[0068] Bind the micro-inertial sensor to the pedestrian's foot to collect the pedestrian's three-axis acceleration and three-axis angular velocity data;

[0069] Perform Fourier transform on the three-axis acceleration and three-axis angular velocity data to obtain the frequency domain signal of each axis;

[0070] According to the maximum amplitude value of the frequency domain signal of each axis in the three-axis acceleration and the three-axis angular velocity, a forgetting factor is constructed, and a weight coefficient is constructed based on the forgetting factor;

[0071] Adaptively estimate the measurement noise covariance matrix in a weighted manner;

[0072] Construct a Kalman filter and use it to estimate navigation errors.

[0073] The present invention is suitable for high-precision positioning and navigation of pedestrians in a variable-step-frequency motion state.

[0074] The technical solution of the present invention is described in detail below in conjunction with a specific embodiment. In order to improve the pedestrian navigation accuracy in actual motion states such as variable step frequency, the present invention proposes a pedestrian navigation adaptive Kalman filtering method based on frequency domain analysis, comprising the following steps:

[0075] 1. Collect inertial data and perform frequency domain transformation

[0076] The micro-inertial sensor is fixed on the foot of the pedestrian to collect the original inertial data of the pedestrian's movement, including three-axis acceleration data and three-axis angular velocity data. The six-axis time domain data is converted to the frequency domain using the Fourier transform method. The Fourier transform formula is as follows:

[0077]

[0078] Among them, f(t) is the original time domain signal, S(ω) is the transformed frequency domain signal, t represents time, and ω represents frequency.

[0079] In the present invention, the three-axis acceleration is the acceleration components of the X, Y, and Z axes of the micro-inertial sensor carrier coordinate system; the three-axis angular velocity is the angular velocity components of the X, Y, and Z axes of the micro-inertial sensor carrier coordinate system.

[0080] 2. Construct forgetting factor and weight coefficient

[0081] After Fourier transforming the original six-axis inertial data, the maximum value of the frequency domain data amplitude of each axis is obtained, and the forgetting factor is constructed as follows:

[0082]

[0083] Among them, a gyr_x 、a gyr_y 、a gyr_z They are the maximum amplitude values ​​of the three-axis angular velocity signals in the frequency domain after Fourier transform;

[0084] a acc_x 、a acc_y 、a acc_z They are the maximum amplitude values ​​of the three-axis acceleration signals in the frequency domain after Fourier transformation;

[0085] α is the forgetting factor.

[0086] The weight coefficient is constructed using the forgetting factor as follows:

[0087]

[0088] Among them, θ k and θ k-1 are the weight coefficients at time k and time k-1 respectively. The initial value of the weight coefficient θ0 is 1.

[0089] The above weight coefficients are used for adaptive updating of the measurement noise covariance matrix during the Kalman filtering process.

[0090] 3. Adaptive Kalman Filter

[0091] The Kalman filter system state variables are as follows:

[0092]

[0093] Among them, δV n , δV u , δV e Represent the velocity errors in the north, sky, and east directions respectively;

[0094] δL, δh, and δλ represent latitude error, altitude error, and longitude error, respectively;

[0095] φ n ,φ u ,φ e Respectively represent the misalignment angles in the north, sky and east directions;

[0096] Respectively represent the accelerometer zero bias in the X, Y, and Z directions;

[0097] ε x , ε y , ε z Respectively represent the gyro drift in the X, Y, and Z directions.

[0098] The system state equation and measurement equation are:

[0099] X k =Φ k,k-1 X k-1 +Γ k-1 w k-1

[0100] Z k =H k X k +v k

[0101] Among them, X k is the system state vector at time k, X k-1 is the system state vector at time k-1; Φ k,k-1 is the state transfer matrix;

[0102] wk-1 is the system noise;

[0103] Γ k-1 is the system noise input matrix;

[0104] Z k is the measurement vector;

[0105] H k is the measurement matrix;

[0106] v k To measure noise.

[0107] And the system noise w at time k k and measurement noise v k Satisfies the following relationship:

[0108]

[0109] Among them, Q k represents the system noise covariance matrix, which is a non-negative definite matrix;

[0110] R k represents the measurement noise covariance matrix, which is a positive definite matrix;

[0111] E[] indicates the calculation of the mean;

[0112] Cov[] represents w k and v k The covariance of

[0113] δ kj is the Kronecker function, that is:

[0114]

[0115] j represents a time different from k.

[0116] The measurement vector Z of the Kalman filter k is the speed error in the zero speed range, that is:

[0117]

[0118] in, They represent the north speed, day speed and east speed at time k respectively.

[0119] The standard Kalman filter process is as follows:

[0120]

[0121] in, Estimate the state vector for one-step forecast;

[0122] is the optimal estimated state vector;

[0123] P k,k-1 is the one-step prediction covariance matrix;

[0124] P k is the optimal estimated covariance matrix;

[0125] K k is the optimal gain matrix;

[0126] I is the unit matrix;

[0127] Φ k,k-1 is the state transfer matrix.

[0128] The R matrix of the standard Kalman filter remains unchanged, but in pedestrian navigation, due to the different cadences of pedestrian movement, the accuracy of zero-speed state determination varies. When the cadence is slow, the accuracy of zero-speed determination is high, and the R matrix is ​​relatively small; when the cadence is fast, the accuracy of zero-speed determination decreases, and the R matrix should be appropriately increased. Therefore, using the weight coefficient θ obtained above k Adaptively update the R matrix. Define the new information as:

[0129]

[0130] Then the adaptive estimate of the R matrix is:

[0131]

[0132] in, is the new information from time k-1 to time k, Z k is the measurement vector; H k is the measurement matrix; I is the unit matrix; θ k is the weight coefficient at time k; P k,k-1 is the one-step prediction covariance matrix; are the adaptive estimates of the measurement noise covariance matrix at time k and time k-1 respectively.

[0133] The present invention uses pedestrian motion characteristics to adaptively adjust filter parameters, uses Fourier transform to extract frequency domain features such as amplitude, constructs forgetting factors, and adaptively estimates the measurement noise covariance matrix in a weighted manner, further improving the error estimation and correction accuracy under actual motion conditions such as variable step frequency. The present invention greatly improves the pedestrian navigation accuracy based on zero-speed correction.

[0134] Features described and / or illustrated above for one embodiment may be used in the same or similar manner in one or more other embodiments, and / or combined with or used in place of features in other embodiments.

[0135] It should be emphasized that the term "include / comprises" when used herein refers to the presence of features, integers, steps or components, but does not exclude the presence or addition of one or more other features, integers, steps, components or combinations thereof.

[0136] The many features and advantages of these embodiments are apparent from this detailed description, and thus the appended claims are intended to cover all such features and advantages of these embodiments that fall within their true spirit and scope. Furthermore, since numerous modifications and changes will readily occur to those skilled in the art, it is not intended that the embodiments of the invention be limited to the exact construction and operation illustrated and described, but rather all suitable modifications and equivalents falling within the scope thereof are intended to be covered.

[0137] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

[0138] Parts of the present invention that are not described in detail are well known to those skilled in the art.

Claims

1. A pedestrian navigation adaptive Kalman filtering method based on frequency domain analysis, characterized in that: The steps include: Bind the micro-inertial sensor to the pedestrian's foot to collect the pedestrian's three-axis acceleration and three-axis angular velocity data; Perform Fourier transform on the three-axis acceleration and three-axis angular velocity data to obtain the frequency domain signal of each axis; According to the maximum amplitude value of the frequency domain signal of each axis in the three-axis acceleration and the three-axis angular velocity, a forgetting factor is constructed, and a weight coefficient is constructed based on the forgetting factor; Adaptively estimate the measurement noise covariance matrix in a weighted manner; Construct a Kalman filter and use it to estimate navigation errors.

2. The method according to claim 1, characterized in that The method for constructing the forgetting factor according to the maximum value of the frequency domain signal amplitude of each axis in the three-axis acceleration and the three-axis angular velocity is as follows: Among them, a gyr_x 、a gyr_y 、a gyr_z They are the maximum amplitude values ​​of the frequency domain signals of the angular velocity signals of the X, Y and Z axes respectively; a acc_x 、a acc_y 、a acc_z They are the maximum amplitude values ​​of the frequency domain signals of the X, Y, and Z axis acceleration signals respectively; α is the forgetting factor.

3. The method according to claim 2, characterized in that The weight coefficient based on the forgetting factor is constructed as follows: Among them, θ k ,θ k-1 are the weight coefficients at time k and time k-1 respectively, and α is the forgetting factor.

4. The method according to claim 3, characterized in that The initial value of the weight coefficient is 1.

5. The method according to claim 3, characterized in that: The method for adaptively estimating the measurement noise covariance matrix in a weighted manner is as follows: Construct the new information as: The adaptive estimation method of the measurement noise covariance matrix R is as follows: in, is the new information from time k-1 to time k; H k is the measurement matrix; I is the unit matrix; θ k is the weight coefficient at time k; P k,k-1 is the one-step prediction covariance matrix; are the adaptive estimates of the measurement noise covariance matrix at time k and time k-1 respectively.

6. The method according to claim 1, characterized in that The steps of constructing the Kalman filter are as follows: The state equation and measurement equation of the Kalman filter system are: X k =Φ k,k-1 X k-1 +C k-1 w k-1 Z k =H k X k +v k Among them, X k is the system state vector at time k, X k-1 is the system state vector at time k-1; Φ k,k-1 is the state transfer matrix; w k-1 is the system noise; Γ k-1 is the system noise input matrix; Z k is the measurement vector; H k is the measurement matrix; v k To measure noise; The measurement vector Z of the Kalman filter k is the speed error in the zero speed range: in, They represent the north speed, day speed and east speed at time k respectively.

7. The method according to claim 6, characterized in that The system noise w at time k k and measurement noise v k Satisfies the following relationship: Among them, Q k represents the system noise covariance matrix, which is a non-negative definite matrix; R k represents the measurement noise covariance matrix, which is a positive definite matrix; E[] indicates the calculation of the mean; Cov[] represents w k and v k The covariance of δ kj is the Kronecker function: j represents a time different from k.

8. The method according to any one of claims 1 to 7, characterized in that: The Kalman filter system state variables are: X=[δV n δV u δV e δLδhδλφ n f u f e ▽ x ▽ y ▽ z e x e y e z ] T Among them, δV n , δV u , δV e Represent the velocity errors in the north, sky, and east directions respectively; δL, δh, and δλ represent latitude error, altitude error, and longitude error, respectively; φ n ,φ u ,φ e Respectively represent the misalignment angles in the north, sky and east directions; ▽ x ,▽ y ,▽ z Respectively represent the accelerometer zero bias in the X, Y, and Z directions; ε x , ε y , ε z Respectively represent the gyro drift in the X, Y, and Z directions.

9. The method according to any one of claims 1 to 7, characterized in that: The Fourier transform method is as follows: Among them, f(t) is the acceleration or angular velocity signal of a certain axis, S(ω) is the transformed frequency domain signal, t represents time, and ω represents frequency.

10. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: The processor executes the computer program to implement the method described in any one of claims 1 to 9.

Images