CN112070170A - Dynamic residual threshold self-adaptive quaternion particle filtering attitude calculation data fusion method - Google Patents

Dynamic residual threshold self-adaptive quaternion particle filtering attitude calculation data fusion method Download PDF

Info

Publication number
CN112070170A
CN112070170A CN202010950223.XA CN202010950223A CN112070170A CN 112070170 A CN112070170 A CN 112070170A CN 202010950223 A CN202010950223 A CN 202010950223A CN 112070170 A CN112070170 A CN 112070170A
Authority
CN
China
Prior art keywords
quaternion
particle
moment
residual
value
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202010950223.XA
Other languages
Chinese (zh)
Other versions
CN112070170B (en
Inventor
周翟和
刘颀绮
游霞
陈则王
田祥瑞
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN202010950223.XA priority Critical patent/CN112070170B/en
Publication of CN112070170A publication Critical patent/CN112070170A/en
Application granted granted Critical
Publication of CN112070170B publication Critical patent/CN112070170B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G06F18/251—Fusion techniques of input or preprocessed data
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2218/00—Aspects of pattern recognition specially adapted for signal processing
    • G06F2218/02—Preprocessing

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • General Physics & Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Mathematical Physics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • General Engineering & Computer Science (AREA)
  • Mathematical Optimization (AREA)
  • Computational Mathematics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Computing Systems (AREA)
  • Evolutionary Biology (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Computation (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Navigation (AREA)

Abstract

The invention provides a dynamic residual threshold self-adaptive quaternion particle filtering attitude calculation data fusion method, belongs to the technical field of digital filtering and multi-sensor data fusion, and is mainly used for improving the accuracy of carrier attitude estimation. The method takes a standard particle filter algorithm as a frame, integrates data of a gyroscope, an accelerometer and a magnetometer, establishes a relatively accurate sensor measurement model through error analysis of a sensor, selects a quaternion as an attitude parameter, carries out prediction and judgment on a reasonable range of residual errors, then adaptively adjusts a measurement noise matrix of a system, and realizes data fusion of the sensor. The method is suitable for a nonlinear attitude measurement system, has good anti-interference performance and attitude calculation precision, and can effectively solve the problem of carrier attitude calculation in a complex environment.

Description

Dynamic residual threshold self-adaptive quaternion particle filtering attitude calculation data fusion method
Technical Field
The invention provides a dynamic residual threshold value self-adaptive quaternion particle filtering attitude calculation data fusion method, belongs to the technical field of digital filtering and multi-sensor data fusion, and is suitable for a nonlinear attitude measurement system.
Background
The attitude measurement system of the carrier is actually a nonlinear system, and in order to realize accurate calculation of the attitude, data of a gyroscope, an accelerometer and a magnetometer needs to be fused by nonlinear filtering, but the accelerometer and the magnetometer are easily interfered by non-gravitational acceleration and a magnetic field, so that output data of the accelerometer and the magnetometer contain more noise, process noise and measurement noise are unknown and have time-varying characteristics, and filtering divergence may be caused by incorrect parameter estimation, so that a proper filtering algorithm needs to be used. Therefore, the dynamic residual threshold value self-adaptive quaternion particle filtering attitude calculation data fusion method is produced, the method can effectively judge the sensor measurement data with larger errors, adaptively adjust the measurement noise, inhibit the interference of non-gravitational acceleration and a magnetic field on attitude estimation, and compared with the traditional algorithm, the algorithm has higher attitude calculation precision.
In the actual motion process of the carrier, the accelerometer and the magnetometer are easily influenced by non-gravity acceleration and the magnetometer, so that the output data error of the accelerometer and the magnetometer is larger, and the filtering effect is influenced.
Disclosure of Invention
The invention aims to provide a dynamic residual threshold value self-adaptive quaternion particle filtering attitude calculation data fusion method (RDT-QPF) aiming at the problems and the defects, the algorithm can analyze the sensor error and establish a more accurate sensor measurement model, the residual threshold value range is predicted, if the residual exceeds the range, a system measurement noise matrix is adjusted, an accurate attitude angle is obtained, and the accurate measurement and the effective control of the attitude in the carrier motion process are ensured.
The invention comprises the following steps:
step 1: selecting a quaternion q ═ q0 qv]TAs quaternion particle filter state quantities, i.e. x ═ q, where q is0And q isvRespectively representing scalar and vector parts of quaternion, establishing a quaternion particle filter state equation according to gyroscope output and a gyroscope error model, and outputting f according to an accelerometerbAnd magnetometer output mbAs observed quantity, establishing a quaternion particle filtering measurement equation;
step 2: filter initialization, obtaining initial quaternion q according to initial attitude angle0Sampling results in a series of quaternion particle sets
Figure BSA0000218352360000011
Initializing the weight of each particle to be 1/N, wherein a superscript i represents a particle index, a subscript 0 represents an initial moment, and N is the total number of the particles;
and step 3: updating time, namely performing self-adaptive quaternion particle filtering state equation on quaternion particle set at the k moment according to the established dynamic residual error threshold value
Figure BSA0000218352360000021
Updating to obtain a prediction quaternion particle set at the moment k +1
Figure BSA0000218352360000022
And 4, step 4: residual mean value mu according to time kkSum variance σk+1 2Predicting dynamic residual error threshold interval at k +1 moment by information, and judging residual error value r at k +1 momentk+1Whether the dynamic residual error threshold interval S- [ mu ] is exceededk+1-σk+1，μk+1+σk+1]If the residual value exceeds the dynamic residual threshold interval, the measurement noise matrix R at the k +1 moment of the system is adjusted in a self-adaptive mannerk+1(ii) a Wherein represents the correlation coefficient, muk+1And σk+1Represents the mean and standard deviation of the residuals at time k + 1;
and 5: carrying out system measurement updating by using a measurement noise matrix obtained by self-adaptive estimation and the measurement equation in the step1, and calculating the importance weight of the particle at the moment of k +1
Figure BSA0000218352360000023
And normalizing;
step 6: resampling if the effective particle number
Figure BSA0000218352360000024
Then a random resampling operation is performed on the set of particles, wherein
Figure BSA0000218352360000025
Represents the weight, N, of the ith particle at time k +1threholdRepresenting a resampled particle threshold;
and 7: carrying out state estimation on the measured and updated particle set to obtain an optimal quaternion estimation value
Figure BSA00002183523600000210
Carrying out normalization processing, and solving by utilizing a quaternion filtering value attitude calculation method to obtain a pitch angle theta, a roll angle gamma and a course angle psi;
and 8: and (5) circulating the step3 to the step 7 until the filtering is finished.
Further, in step1, a quaternion is defined as q ═ q0 qv]T＝[q0 q1 q2 q3]TWherein q is0And q isvRepresenting quaternion scalar and vector parts, respectively, q1～q3Three vector elements of quaternion, the quaternion kinematic differential equation is:
Figure BSA0000218352360000026
wherein the content of the first and second substances,
Figure BSA0000218352360000027
denotes the multiplication of quaternions, q (t) denotes the quaternion matrix at time t, ω (t) denotes the angular velocity at time t, assuming that the angular velocity vector is in the form of a quaternion, Φω(t)A quaternion multiplication exchange matrix composed of angular velocities;
solving the quaternion kinematic differential equation and discretizing to obtain:
Figure BSA0000218352360000028
define Θ (T) content as follows:
Figure BSA0000218352360000029
where θ (T) represents the triaxial angular velocity increment over a [0, T ] period, i.e.:
Figure BSA0000218352360000031
the system state quantity x is thus chosen as follows:
x＝q＝[q0 q1 q2 q3]T
the dynamic residual threshold value self-adaptive quaternion particle filtering state equation is established as follows:
Figure BSA0000218352360000032
where Δ T represents a sampling time interval;
accelerometer output data fbIn relation to the conditions of motion of a four-rotor aircraft, whichThe output model can be expressed as:
Figure BSA0000218352360000033
in the formula (I), the compound is shown in the specification,
Figure BSA0000218352360000034
is an attitude transformation matrix from a navigation coordinate system (n system) to a carrier coordinate system (b system), gnIs the local gravity component, e is the non-gravitational acceleration, nfIs the gaussian white noise of the accelerometer,
Figure BSA0000218352360000035
the specific form is as follows:
Figure BSA0000218352360000036
magnetometer output of mbThe model can be expressed as:
Figure BSA0000218352360000037
in the formula, mnIs the local magnetic field strength component, nmIn the case of the magnetometer white gaussian noise,
selecting a system observed quantity z as follows:
Figure BSA0000218352360000038
wherein the content of the first and second substances,
Figure BSA0000218352360000039
representing the triaxial output components of the accelerometer and magnetometer, respectively;
establishing a dynamic residual threshold adaptive quaternion particle filtering observation equation as follows:
Figure BSA00002183523600000310
wherein h (-) represents a non-linear function of the system observation equation, vkRepresenting the observed noise of the system;
covariance matrix:
Figure BSA00002183523600000311
wherein R isfAnd RmNoise covariance matrices for accelerometer and magnetometer, 0, respectively3×3A zero matrix of 3 × 3;
further, in step2, an initial quaternion q is obtained according to the initial attitude angle0Sampling results in a series of quaternion particle sets
Figure BSA0000218352360000041
Initializing the weight of each particle to be 1/N, wherein a superscript i represents a particle index, a subscript 0 represents an initial moment, and N is the total number of the particles;
initial quaternion value q0Can be determined from the initial attitude angle:
Figure BSA0000218352360000042
wherein theta represents a pitch angle, gamma represents a roll angle, and psi represents a heading angle;
further, in step3, the quaternion particle set at the k moment is subjected to the filtering state equation of the quaternion particle with the dynamic residual threshold value self-adaption established
Figure BSA0000218352360000043
Updating to obtain a prediction quaternion particle set at the moment k +1
Figure BSA0000218352360000044
Further, step 4 specifically includes the following steps:
step 1: root of herbaceous plantMean value of residual errors mu according to time kkSum variance σk 2Information, predicting a k +1 moment dynamic residual threshold interval:
residual mean value mu of k +1 time predictionk+1Sum variance σk+1 2The calculation formula is as follows:
Figure BSA0000218352360000045
wherein r isk+1Is the residual value at the time of k +1,
Figure BSA0000218352360000046
residual value of ith particle at time k + 1:
Figure BSA0000218352360000047
wherein z isk+1Is the measured value of the system at the moment k +1,
Figure BSA0000218352360000048
the predicted measurement value of the ith particle at the moment k + 1;
step 2: judging the residual value r at the k +1 momentk+1Whether the dynamic residual error threshold interval S- [ mu ] is exceededk+1-σk+1，μk+1+σk+1]Wherein denotes a correlation coefficient, μk+1And σk+1Represents the mean and standard deviation of the residuals at time k + 1;
step 3: if the residual error value exceeds the dynamic residual error threshold interval, the measurement noise matrix R at the k +1 moment of the system is adjusted in a self-adaptive mannerk+1Introduction of residual information pairs R within the filter window hk+1And (3) adjusting:
Figure BSA0000218352360000051
wherein h represents the width of the filtering window;
further, the step 5 is estimated by using self-adaptionCarrying out system measurement updating on the measured noise matrix and the measurement equation in the step1, and weighting the importance of the particles at the moment of k +1
Figure BSA0000218352360000052
Can be calculated from the following equation:
Figure BSA0000218352360000053
normalization of particle weight:
Figure BSA0000218352360000054
further, the effective particle number is defined in step 6
Figure BSA0000218352360000055
If N is presenteff＜NthreholdThen a random resampling operation is performed on the set of particles, wherein
Figure BSA0000218352360000056
Represents the weight, N, of the ith particle at time k +1threholdRepresenting a resampled particle threshold;
further, in step 7, performing state estimation on the updated measured particle set to obtain an optimal quaternion estimation value, performing normalization processing, and solving by using a quaternion filtering value attitude solution method to obtain an attitude angle.
The following formula is adopted to obtain the estimated value of the optimal quaternion
Figure BSA0000218352360000057
Figure BSA0000218352360000058
The formula shows that the optimal quaternion estimated value is the eigenvector corresponding to the maximum eigenvalue of the matrix M;
the pitch angle theta and the roll angle can be obtained from the optimal quaternion estimated valueGamma and course angle
Figure BSA0000218352360000059
The calculation formula is as follows:
Figure BSA00002183523600000510
further, step 8, the step3 to the step 7 are circulated until the filtering is finished.
The invention has the following advantages: according to the method, on the basis of a standard particle filter algorithm, quaternions are selected as attitude parameters, the threshold value of a residual error is predicted, and then the measurement noise matrix of the system is adjusted in a self-adaptive manner, so that the stability of the filter algorithm is ensured, the attitude estimation precision can be effectively improved, and an accurate attitude angle can be still obtained under the condition that an accelerometer and a magnetometer are interfered by non-gravity acceleration and a magnetic field.
Drawings
FIG. 1 is a flow chart of a dynamic residual threshold adaptive quaternion particle filtering attitude calculation data fusion method
FIG. 2 residual and dynamic threshold discrimination
FIG. 3 attitude solution angle error comparison
Detailed Description
The following detailed description of embodiments of the invention is intended to be illustrative, but not limiting, of the invention. The dynamic residual threshold value adaptive quaternion particle filtering attitude calculation data fusion method is described in detail below with reference to the attached drawings of the specification.
In order to better embody the specific steps, implementation and effects of the invention, the following simulation experiment is set up: the performance of the algorithm was verified using a MATLAB simulation platform. In the experimental environment, the following measurement models can be set up:
the gyroscope output model may be represented as ωbω +, where ω is the true angular velocity,the output error of the gyroscope mainly comprises three components: is ═ ib+r+ngWherein, in the step (A),bin order to be a random constant drift,rfor the first order markov process of the gyroscope,
Figure BSA0000218352360000061
Tgis the correlation time, ωrIs Markov process white noise, ngIs white gaussian noise from a gyroscope.
The output model of the accelerometer and magnetometer is:
Figure BSA0000218352360000062
in the formula
Figure BSA0000218352360000063
Is an attitude transformation matrix from a navigation coordinate system (n system) to a carrier coordinate system (b system), gnIs the local gravity component, e is the non-gravitational acceleration, nfIs Gaussian white noise of accelerometer, mnIs the local magnetic field strength component, nmIs magnetometer gaussian white noise.
The simulation parameters are set as follows: the number of particles is N1000, the simulation duration is 60s, the sampling frequency of the sensor is set to 100Hz, and the noise coefficient of the triaxial gyroscope is sigmaω0.2778, the three-axis accelerometer noise figure is σf0.124, noise coefficient of the three-axis magnetometer is σm＝0.244。
For the above simulation experiment data, according to fig. 1, a flow chart of a dynamic residual threshold adaptive quaternion particle filtering attitude calculation data fusion method is implemented, and the specific steps are as follows:
step 1: selecting a quaternion q ═ q0 qv]TAs quaternion particle filter state quantities, i.e. x ═ q, where q is0And q isvRespectively representing scalar and vector parts of quaternion, establishing a quaternion particle filter state equation according to gyroscope output and a gyroscope error model, and outputting f according to an accelerometerbAnd magnetometer output mbAs observed quantity, establishing a quaternion particle filtering measurement equation;
define quaternion as q ═ q0 qv]T＝[q0 q1 q2 q3]TWherein q is0And q isvRepresenting quaternion scalar and vector parts, respectively, q1～q3Three vector elements of quaternion, the quaternion kinematic differential equation is:
Figure BSA0000218352360000071
wherein the content of the first and second substances,
Figure BSA0000218352360000072
denotes the multiplication of quaternions, q (t) denotes the quaternion matrix at time t, ω (t) denotes the angular velocity at time t, assuming that the angular velocity vector is in the form of a quaternion, Φω(t)A quaternion multiplication exchange matrix composed of angular velocities;
solving the quaternion kinematic differential equation and discretizing to obtain:
Figure BSA0000218352360000073
define Θ (T) content as follows:
Figure BSA0000218352360000074
where θ (T) represents the triaxial angular velocity increment over a [0, T ] period, i.e.:
Figure BSA0000218352360000075
the system state quantity x is thus chosen as follows:
x＝q＝[q0 q1 q2 q3]T
the dynamic residual threshold value self-adaptive quaternion particle filtering state equation is established as follows:
Figure BSA0000218352360000076
where Δ T represents a sampling time interval;
accelerometer output data fbIn relation to the motion conditions of a quad-rotor aircraft, its output model can be expressed as:
Figure BSA0000218352360000077
in the formula (I), the compound is shown in the specification,
Figure BSA0000218352360000078
is an attitude transformation matrix from a navigation coordinate system (n system) to a carrier coordinate system (b system), gnIs the local gravity component, e is the non-gravitational acceleration, nfIs the gaussian white noise of the accelerometer,
Figure BSA0000218352360000079
the specific form is as follows:
Figure BSA00002183523600000710
magnetometer output of mbThe model can be expressed as:
Figure BSA00002183523600000711
in the formula, mnIs the local magnetic field strength component, nmIn the case of the magnetometer white gaussian noise,
selecting a system observed quantity z as follows:
Figure BSA0000218352360000081
wherein the content of the first and second substances,
Figure BSA0000218352360000082
representing the triaxial output components of the accelerometer and magnetometer, respectively;
establishing a dynamic residual threshold adaptive quaternion particle filtering observation equation as follows:
Figure BSA0000218352360000083
wherein h (-) represents a non-linear function of the system observation equation, vkRepresenting the observed noise of the system;
covariance matrix:
Figure BSA0000218352360000084
wherein R isfAnd RmNoise covariance matrices for accelerometer and magnetometer, 0, respectively3×3A zero matrix of 3 × 3;
step 2: filter initialization, obtaining initial quaternion q according to initial attitude angle0Sampling results in a series of quaternion particle sets
Figure BSA0000218352360000085
Initializing the weight of each particle to be 1/N, wherein a superscript i represents a particle index, a subscript 0 represents an initial moment, and N is the total number of the particles;
in this example, the initial attitude angle is set to Λ0＝[0° 0° 30°]TInitial quaternion value q0Can be determined from the initial attitude angle:
Figure BSA0000218352360000086
wherein theta represents a pitch angle, gamma represents a roll angle, and psi represents a heading angle;
and step 3: updating time, and self-adapting quaternion particle filtering state equation pair k according to the established dynamic residual error threshold valueQuaternion particle set of time of day
Figure BSA0000218352360000087
Updating to obtain a prediction quaternion particle set at the moment k +1
Figure BSA0000218352360000088
And 4, step 4: residual mean value mu according to time kkSum variance σk 2Predicting dynamic residual error threshold interval at k +1 moment by information, and judging residual error value r at k +1 momentk+1Whether the dynamic residual error threshold interval S- [ mu ] is exceededk+1-σk+1，μk+1+σk+1]If the residual value exceeds the dynamic residual threshold interval, the measurement noise matrix R at the k +1 moment of the system is adjusted in a self-adaptive mannerk+1(ii) a Wherein represents the correlation coefficient, muk+1And σk+1The method represents the mean value and the standard deviation of the residual error at the moment k +1, and specifically comprises the following steps:
step 1: residual mean value mu according to time kkSum variance σk+1 2Information, predicting a k +1 moment dynamic residual threshold interval:
residual mean value mu of k +1 time predictionk+1Sum variance σk+1 2The calculation formula is as follows:
Figure BSA0000218352360000091
wherein r isk+1Is the residual value at the time of k +1,
Figure BSA0000218352360000092
residual value of ith particle at time k + 1:
Figure BSA0000218352360000093
wherein z isk+1Is the measured value of the system at the moment k +1,
Figure BSA0000218352360000094
the predicted measurement value of the ith particle at the moment k + 1;
step 2: judging the residual value r at the k +1 momentk+1Whether the dynamic residual error threshold interval S- [ mu ] is exceededk+1-σk+1，μk+1+σk+1]Wherein denotes a correlation coefficient, μk+1And σk+1Represents the mean and standard deviation of the residuals at time k + 1;
step 3: if the residual error value exceeds the dynamic residual error threshold interval, the measurement noise matrix R at the k +1 moment of the system is adjusted in a self-adaptive mannerk+1Introduction of residual information pairs R within the filter window hk+1And (3) adjusting:
Figure BSA0000218352360000095
wherein h represents the width of the filtering window;
and 5: carrying out system measurement updating by using a measurement noise matrix obtained by self-adaptive estimation and the measurement equation in the step1, and calculating the importance weight of the particle at the moment of k +1
Figure BSA0000218352360000096
And normalizing;
importance weight of particle at time k +1
Figure BSA0000218352360000097
Can be calculated from the following equation:
Figure BSA0000218352360000098
normalization of particle weight:
Figure BSA0000218352360000099
step 6: resampling if the effective particle number
Figure BSA00002183523600000910
Then particle alignment is carried outThe subsets are subjected to a random resampling operation, wherein
Figure BSA00002183523600000911
Represents the weight, N, of the ith particle at time k +1threholdRepresenting the resampled particle threshold, N in this examplethrehold＝2N/3；
And 7: carrying out state estimation on the measured and updated particle set to obtain an optimal quaternion estimation value
Figure BSA0000218352360000101
Carrying out normalization processing, and solving by utilizing a quaternion filtering value attitude calculation method to obtain a pitch angle theta, a roll angle gamma and a course angle psi;
the following formula is adopted to obtain the estimated value of the optimal quaternion
Figure BSA0000218352360000102
Figure BSA0000218352360000103
The formula shows that the optimal quaternion estimated value is the eigenvector corresponding to the maximum eigenvalue of the matrix M;
the pitch angle theta, roll angle gamma and course angle can be obtained from the optimal quaternion estimated value
Figure BSA0000218352360000104
The calculation formula is as follows:
Figure BSA0000218352360000105
and 8: and (5) circulating the step3 to the step 7 until the filtering is finished.
And analyzing the effect of the method, adding non-gravity acceleration interference to the Y axis of the accelerometer within 25-30 s in the simulation process, and adding magnetic field interference to the three axes of the magnetometer. As shown in fig. 2, acc-x, acc-y, acc-z, mag-x, mag-y, and mag-z of the y-axis respectively represent triaxial output residual values of the accelerometer and the magnetometer, and the residual values in the corresponding time period all exceed the prediction threshold range, which indicates that the error of the measured data of the sensor is large at this time, as shown in fig. 3, the fluctuation of the pitch angle pitch, roll angle roll, and course angle yaw obtained by using unscented kalman filter algorithm (UKF) and standard particle filter algorithm (QPF) is large, and a dynamic residual threshold adaptive quaternion particle filter attitude data fusion method (RDT-QPF) has a better suppression effect on external interference through the adaptive adjustment of the measured noise covariance matrix of the system, and the filtering effect is obviously better than that of the other two algorithms, thereby improving the accuracy of attitude calculation.

Claims (2)

1. A dynamic residual threshold value self-adaptive quaternion particle filtering attitude calculation data fusion method is characterized by comprising the following steps:
step 1: selecting a quaternion q ═ q0 qv]TAs quaternion particle filter state quantities, i.e. x ═ q, where q is0And q isvRespectively representing scalar and vector parts of quaternion, establishing a quaternion particle filter state equation according to gyroscope output and a gyroscope error model, and outputting f according to an accelerometerbAnd magnetometer output mbAs observed quantity, establishing a quaternion particle filtering measurement equation;
step 2: filter initialization, obtaining initial quaternion q according to initial attitude angle0Sampling results in a series of quaternion particle sets
Figure FSA0000218352350000011
Initializing the weight of each particle to be 1/N, wherein a superscript i represents a particle index, a subscript 0 represents an initial moment, and N is the total number of the particles;
and step 3: updating time, namely performing self-adaptive quaternion particle filtering state equation on quaternion particle set at the k moment according to the established dynamic residual error threshold value
Figure FSA0000218352350000012
Updating to obtain a prediction quaternion particle set at the moment k +1
Figure FSA0000218352350000013
And 4, step 4: residual mean value mu according to time kkSum variance σk 2Predicting dynamic residual error threshold interval at k +1 moment by information, and judging residual error value r at k +1 momentk+1Whether the dynamic residual error threshold interval S- [ mu ] is exceededk+1-σk+1，μk+1+σk+1]If the residual value exceeds the dynamic residual threshold interval, the measurement noise matrix R at the k +1 moment of the system is adjusted in a self-adaptive mannerk+1(ii) a Wherein represents the correlation coefficient, muk+1And σk+1Represents the mean and standard deviation of the residuals at time k + 1;
and 5: carrying out system measurement updating by using a measurement noise matrix obtained by self-adaptive estimation and the measurement equation in the step1, and calculating the importance weight of the particle at the moment of k +1
Figure FSA0000218352350000014
And normalizing;
step 6: resampling if the effective particle number
Figure FSA0000218352350000015
Then a random resampling operation is performed on the set of particles, wherein
Figure FSA0000218352350000016
Represents the weight, N, of the ith particle at time k +1threholdRepresenting a resampled particle threshold;
and 7: carrying out state estimation on the measured and updated particle set to obtain an optimal quaternion estimation value
Figure FSA0000218352350000017
Carrying out normalization processing, and solving by utilizing a quaternion filtering value attitude calculation method to obtain a pitch angle theta, a roll angle gamma and a course angle psi;
and 8: and (5) circulating the step3 to the step 7 until the filtering is finished.
2. The dynamic residual threshold adaptive quaternion particle filter attitude calculation data fusion method of claim 1, characterized in that in step 4, the residual mean μ at time k is used as the referencekSum variance σk 2Predicting dynamic residual error threshold interval at k +1 moment by information, and judging residual error value r at k +1 momentk+1Whether the dynamic residual error threshold interval S- [ mu ] is exceededk+1-σk+1，μk+1+σk+1]If the residual value exceeds the dynamic residual threshold interval, the measurement noise matrix R at the k +1 moment of the system is adjusted in a self-adaptive mannerk+1(ii) a The method specifically comprises the following steps:
step 1: residual mean value mu according to time kkSum variance σk 2Information, predicting a k +1 moment dynamic residual threshold interval:
residual mean value mu of k +1 time predictionk+1Sum variance σk+1 2The calculation formula is as follows:
Figure FSA0000218352350000021
wherein the content of the first and second substances,
Figure FSA0000218352350000022
residual value of ith particle at time k + 1:
Figure FSA0000218352350000023
wherein z isk+1Is the measured value of the system at the moment k +1,
Figure FSA0000218352350000024
the predicted measurement value of the ith particle at the moment k + 1;
step 2: judging the residual value r at the k +1 momentk+1Whether the dynamic residual error threshold interval S- [ mu ] is exceededk+1-σk+1，μk+1+σk+1]WhereinDenotes the correlation coefficient, μk+1And σk+1Mean and standard deviation of the residuals;
step 3: if the residual error value exceeds the dynamic residual error threshold interval, the measurement noise matrix R at the k +1 moment of the system is adjusted in a self-adaptive mannerk+1Introduction of residual information pairs R within the filter window hk+1And (3) adjusting:
Figure FSA0000218352350000025
where h denotes the width of the filter window.
CN202010950223.XA 2020-09-10 2020-09-10 Dynamic residual error threshold self-adaptive quaternion particle filtering attitude calculation data fusion method Active CN112070170B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010950223.XA CN112070170B (en) 2020-09-10 2020-09-10 Dynamic residual error threshold self-adaptive quaternion particle filtering attitude calculation data fusion method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010950223.XA CN112070170B (en) 2020-09-10 2020-09-10 Dynamic residual error threshold self-adaptive quaternion particle filtering attitude calculation data fusion method

Publications (2)

Publication Number Publication Date
CN112070170A true CN112070170A (en) 2020-12-11
CN112070170B CN112070170B (en) 2024-03-29

Family

ID=73663644

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010950223.XA Active CN112070170B (en) 2020-09-10 2020-09-10 Dynamic residual error threshold self-adaptive quaternion particle filtering attitude calculation data fusion method

Country Status (1)

Country Link
CN (1) CN112070170B (en)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112650281A (en) * 2020-12-14 2021-04-13 一飞(海南)科技有限公司 Multi-sensor tri-redundancy system, control method, unmanned aerial vehicle, medium and terminal
CN113124863A (en) * 2021-04-15 2021-07-16 南京航空航天大学 Mixed particle federated filtering data processing method based on KLD sampling
CN115290185A (en) * 2022-09-05 2022-11-04 江苏徐工国重实验室科技有限公司 Anti-vibration filtering method and device and engineering machinery

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108318038A (en) * 2018-01-26 2018-07-24 南京航空航天大学 A kind of quaternary number Gaussian particle filtering pose of mobile robot calculation method
CN109916398A (en) * 2019-03-22 2019-06-21 南京航空航天大学 A kind of adaptive quaternary number particle filter attitude data fusion method

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108318038A (en) * 2018-01-26 2018-07-24 南京航空航天大学 A kind of quaternary number Gaussian particle filtering pose of mobile robot calculation method
CN109916398A (en) * 2019-03-22 2019-06-21 南京航空航天大学 A kind of adaptive quaternary number particle filter attitude data fusion method

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112650281A (en) * 2020-12-14 2021-04-13 一飞(海南)科技有限公司 Multi-sensor tri-redundancy system, control method, unmanned aerial vehicle, medium and terminal
CN112650281B (en) * 2020-12-14 2023-08-22 一飞(海南)科技有限公司 Multi-sensor three-redundancy system, control method, unmanned aerial vehicle, medium and terminal
CN113124863A (en) * 2021-04-15 2021-07-16 南京航空航天大学 Mixed particle federated filtering data processing method based on KLD sampling
CN115290185A (en) * 2022-09-05 2022-11-04 江苏徐工国重实验室科技有限公司 Anti-vibration filtering method and device and engineering machinery

Also Published As

Publication number Publication date
CN112070170B (en) 2024-03-29

Similar Documents

Publication Publication Date Title
CN109974714B (en) Sage-Husa self-adaptive unscented Kalman filtering attitude data fusion method
CN112070170A (en) Dynamic residual threshold self-adaptive quaternion particle filtering attitude calculation data fusion method
CN108827299B (en) Aircraft attitude calculation method based on improved quaternion second-order complementary filtering
CN108362282B (en) Inertial pedestrian positioning method based on self-adaptive zero-speed interval adjustment
CN108197350B (en) Unmanned ship speed and uncertainty estimation system and design method
CN111024064B (en) SINS/DVL combined navigation method for improving Sage-Husa adaptive filtering
CN110231029B (en) Underwater robot multi-sensor fusion data processing method
CN108519090B (en) Method for realizing double-channel combined attitude determination algorithm based on optimized UKF algorithm
CN108731676B (en) Attitude fusion enhanced measurement method and system based on inertial navigation technology
CN111398631A (en) Unmanned aerial vehicle accelerometer error identification and correction method
CN115793453A (en) Self-adaptive control method for tracking of rotorcraft by fusing AI deep learning
CN111649747A (en) IMU-based adaptive EKF attitude measurement improvement method
CN116147624B (en) Ship motion attitude calculation method based on low-cost MEMS navigation attitude reference system
CN113155129A (en) Holder attitude estimation method based on extended Kalman filtering
CN115046545A (en) Positioning method combining deep network and filtering
CN113175926B (en) Self-adaptive horizontal attitude measurement method based on motion state monitoring
CN107860382B (en) Method for measuring attitude by applying AHRS under geomagnetic anomaly condition
CN112179342A (en) Method and system for estimating attitude of unmanned aerial vehicle
CN112683261A (en) Unmanned aerial vehicle robustness navigation method based on speed prediction
CN112729348B (en) Gesture self-adaptive correction method for IMU system
CN110375773B (en) Attitude initialization method for MEMS inertial navigation system
CN110672127B (en) Real-time calibration method for array type MEMS magnetic sensor
CN110048693B (en) Quaternion distribution-based parallel Gaussian particle filter data processing method
CN109443393B (en) Strapdown inertial navigation signal extraction method and system based on blind separation algorithm
CN115793449B (en) Helicopter flight attitude controller design method and device, equipment and storage medium

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant