Quaternion particle filtering attitude estimation method based on hyper-hemisphere uniform deterministic sampling

Abstract

The invention provides a quaternion particle filter attitude estimation method based on hyper-hemisphere uniform deterministic sampling, belongs to the technical field of data processing, and is mainly used for solving the problems of poor real-time performance and large calculated amount of a particle filter in attitude estimation. Meanwhile, the influence of particle depletion and degradation on the attitude estimation algorithm can be avoided. According to the method, quaternion particle filtering is used as a framework, hyper-hemisphere uniform deterministic sampling is embedded, samples uniformly sampled in a hemisphere are mirrored to the whole sphere, quaternion samples are obtained in an exponential mapping mode, traditional random samples are replaced by deterministic samples, weights are updated according to measured values and a weight calculation formula, and a quaternion particle filtering model is obtained. The calculation cost is lower while the filtering precision is guaranteed, and the method is suitable for application occasions such as attitude estimation of a nonlinear system and sensor data fusion.

Description

Technical Field

[0001] The present invention proposes a quaternion particle filter attitude estimation method based on hyper-hemispherical uniform deterministic sampling, which belongs to the field of data processing technology and involves nonlinear filtering. The method provided by the present invention is suitable for application scenarios such as attitude estimation of nonlinear dynamic systems and sensor data fusion. Background Art

[0002] Nonlinear filtering problems arise in many fields, including target tracking, strapdown inertial navigation systems, and aircraft attitude estimation. However, for a strongly nonlinear system such as aircraft attitude estimation, accurate and stable sequential attitude estimation is not easy. In order to reduce the deviation caused by the inevitable drift of the gyroscope and achieve long-term stable estimation, sensor data fusion algorithms are indispensable. IMU attitude estimation is defined in a special orthogonal rotation group called SO(3), whose nonlinear structure is the main problem faced by quaternion filtering. Using quaternions as attitude parameters not only avoids the gimbal deadlock problem of Euler angles, but also represents the double coverage structure of SO(3) with only one degree of redundancy. Quaternion estimation methods can be divided into two types, namely indirect estimation and direct estimation. Indirect estimation uses attitude error parameters as the state vector and relies on local nonlinear linearization assuming that the noise is Gaussian distributed, such as extended Kalman filtering and unscented quaternion filtering, but is limited by the basic problems of local parameterization and linearization inheritance; direct estimation methods use unit quaternions as state vectors. Unit quaternions constitute a good representation of the included double-covered rotation group and have only one redundancy, which can improve data processing performance while ensuring accuracy.

[0003] The direct method has better performance in attitude estimation accuracy and convergence speed than the indirect method, but it usually requires a larger amount of calculation. The deterministic sampling scheme can well improve this shortcoming. Therefore, Dirac mixing is used for the deterministic samples on the hyperhemisphere to simulate the uncertainty of the unit quaternion manifold in a non-parametric form. At the same time, due to the antipodal symmetry of the unit quaternion, any number of deterministic samples can be obtained on the hyperhemisphere, and projected to the unit quaternion manifold through mirroring and exponential mapping to further reduce the amount of calculation. However, at present, in quaternion particle filtering, there is still little research on the deterministic sampling method based on hyperhemisphere sampling. Summary of the invention

[0004] In the present invention, a quaternion particle filter attitude estimation method based on hyperhemispheric uniform deterministic sampling is proposed, referred to as hyperhemispheric quaternion particle filter (HHQPF), which is used for attitude estimation and gyroscope bias. The method is based on discrete quaternion particle filtering, uses unit quaternions as particles in the filter to retain the unit norm constraint of the quaternion, obtains a sample set based on hyperhemispheric uniform deterministic sampling, and the discrete deterministic samples are obtained by exponential mapping the uniform samples obtained by sampling on the hypersphere. Compared with genetic algorithm embedded quaternion particle filter (GA-QPF) and unscented quaternion estimator (USQUE), HHQPF has a lower computational cost while ensuring filtering accuracy.

[0005] The specific process of the present invention is as follows:

[0006] Step 1: Establish a quaternion differential equation as the state equation based on the gyroscope data output, establish the system observation equation based on the output of the accelerometer and magnetometer, select the quaternion q and the gyroscope error β as the state quantity in the system, and the state equation is defined as: in yes The antisymmetric matrix of is the output value of the gyroscope, and the observation equation is defined as: in is the rotation matrix, r k is the observation vector, v k represents the Gaussian white noise of the sensor;

[0007] Step 2: Particle initialization, obtain the initial quaternion based on the initial accelerometer value g0 and the initial magnetometer value m0 Obtain the gyroscope deviation particle set based on the initial gyroscope deviation β0

[0008] Step 3: Hyperhemispheric uniform deterministic sampling, using hyperhemispheric uniform deterministic sampling to generate the particle set at time k It is used for measurement update and recursive estimation, where the process of obtaining the sample set by super-hemispheric uniform deterministic sampling is as follows:

[0009] Step 1: Define the updated estimated a priori optimal quaternion as the projection tangent point q on the manifold k ;

[0010] Step 2: Obtain a uniform sample point set on the super hemisphere through the uniform hemispherical meshing algorithm (HEQ)

[0011] Step 3: Set the sample Through nonlinear mapping (Hemispherical mapping), we get a mirror-symmetric uniform sample set in the lower hemisphere.

[0012] Step 4: Set the sample points after mirroring Project the quaternion sample set x onto the unit quaternion manifold with resolution K;

[0013] Step 5: Output in q k A deterministic sample set centered on

[0014] Step 4: Measurement update, using the particle sample set q at time k k (i) and the measured value y at time k k Calculate the particle weight at time k And normalize in is the weight of the i-th particle at time k, w k (i) is the normalized weight of the particle, and then according to the particle sample set q at time k k (i) and the normalized weight w k (i) Calculate the optimal estimate at time k Also calculates the mean of the gyroscope bias noise and variance And use the mean of the bias noise and variance Get the bias noise β k The posterior distribution of

[0015] Step 5: Time update, combining the particle sample set q at time k k (i) Biased noise posterior distribution and the gyroscope measurement at time k+1 Update the prior particle set at time k+1 and the a priori gyroscope bias noise distribution According to the particle set at time k+1 and the normalized weight w at time k k (i) Calculate the prior estimate at time k+1 According to the calculated prior estimate at time k+1

[0016] Step 6: Loop through steps 3 to 5 to get the filter value at the next moment.

[0017] Furthermore, in step 1, the quaternion is defined as q≡[q1 ρ T ] T, where T represents the matrix transpose; q1 = cos(θ / 2), is the rotation axis and θ is the rotation angle.

[0018] The angular velocity measurement sensor model used is shown below:

[0019]

[0020] in represents the continuous measured angular velocity vector, ω(t) represents the true angular velocity vector, β(t) represents the random walk noise, and ε v (t) and ε w (t) represent independent Gaussian white noise processes:

[0021]

[0022] where δ(t-τ) is a Dirac mixing process, σ v and σ w Represents ε v and ε w The standard deviation of the angular velocity measurement sensor model is as follows:

[0023]

[0024] β k =β k-1 +Δtε wk

[0025] Where Δt represents the sampling interval.

[0026] Remember q k is the quaternion at time k, q k+1 is the quaternion at time k+1, then the system state equation is:

[0027]

[0028] in, is the output value of the gyroscope.

[0029] The observation equation is defined as: in is the rotation matrix, r k is the observation vector, v k represents the Gaussian white noise of the sensor, where the observation quantity r at time k k Composed of the accelerometer output and the magnetometer output, the observation equation of the system can be written as:

[0030]

[0031] in, For navigation coordinate system To the body coordinate system The posture matrix, g n is the local gravitational acceleration, m n is the local magnetic field component; v0 and m0 are the zero-mean white noise of the accelerometer and the zero-mean white noise of the magnetometer, respectively.

[0032] Furthermore, in step 2, the initial bias noise distribution of the gyroscope is obtained. The bias noise of the gyroscope obeys a zero-mean Gaussian distribution. The initial particle distribution of β0 is assumed to be: Where N(·) is a Gaussian distribution.

[0033] Furthermore, in step 3, the particle set at time k is obtained by using hyperhemispheric uniform deterministic sampling The process is as follows:

[0034] Step 1: Define the estimated optimal quaternion as the projection tangent point q on the manifold k ;

[0035] Step 2: Obtain a uniform sample point set on the super hemisphere through the uniform hemispherical meshing algorithm (HEQ)

[0036] Step 3: Sample Set Through nonlinear mapping (Hemispherical mapping), we get a mirror-symmetric uniform sample set in the lower hemisphere.

[0037] Step 4: Set the sample points after mirroring The quaternion sample set x is obtained by projecting it onto the unit quaternion manifold with resolution K, as follows:

[0038]

[0039] in, is the prior estimate, K is the projection resolution,

[0040] Step 5: Output in q k A deterministic sample set centered on

[0041] Furthermore, in Step 2 of the hyper-hemispheric uniform deterministic sampling in Step 3, a uniform sample point set on the hyper-hemispheric is obtained by the uniform hemispheric meshing algorithm (HEQ) The process is as follows:

[0042] (1) Based on and θ c =Θ(V R ) Determine the hypersphere north pole cap boundary θc , where V R is the area of ​​the circular cap, Ω is the area of ​​the unit hyperhemisphere, N is the number of grid divisions, Θ(·) is the inverse function of the spherical area calculation function V(·);

[0043] (2) Based on Get the ideal torus angle δ I , d is the dimension, in this sampling method d = 2;

[0044] (3) Based on Get the ideal number of tori n I , and round down to get the actual number of tori n;

[0045] (4) Based on Get the ideal number of torus grids y i , and round down to get the actual number of torus grids y, where V(·) is the spherical area calculation function, θ F is an increasing sequence of torus dimensions, which can be obtained by get;

[0046] (5) According to the above process, the representation of each grid area is obtained as R = ((τ1, ..., τ d ), (υ1…,υ d )), where (τ1,…,τ d ) and (υ1…,υ d ) is a one-element array represented in polar coordinates;

[0047] (6) Use the center point of each grid area as a sample point to obtain a uniform sample point set on the super hemisphere

[0048] Furthermore, in step 4, the particle sample set q at time k is updated. k (i) and the measured value y at time k k Calculate the particle weight at time k And normalize to get w k (i) The process is as follows:

[0049]

[0050] Furthermore, in step 4, the particle sample set q at time k is updated. k (i) and the normalized weight w k (i) Calculate the optimal estimate at time k Prior Particle and the weight at time k Estimate the central quaternion at time k The central quaternion at time k Equal to the quaternion second moment P qqk The maximum eigenvalue λ max The corresponding unit normalized eigenvector is as follows:

[0051]

[0052] Where p(q k (i)|data) represents the sample q k (i) The corresponding posterior probability weight, and then calculate the posterior mean of β at the current moment and covariance as follows:

[0053]

[0054] Therefore, β k The posterior distribution of is approximately

[0055] Furthermore, in step 5, the particle set at time k+1 is updated. and bias noise distribution And calculate the prior estimate at time k+1 The process is as follows:

[0056] Step 1: From k time q k Update to get k+1 time q k+1 , and generate particle distribution as follows:

[0057]

[0058] in represents the orthogonal transfer matrix, is the angular velocity at time k.

[0059] Step 2: Predict the distribution p(q k+1|k |y 0：k ) can be expressed as:

[0060]

[0061] Where p(q k |y 0：k ) is approximately a Dirac mixture, p(q k+1 |q k )Use weighted particles approximate:

[0062]

[0063] Step 3: From gyro bias The approximate predictive distribution of is calculated as follows:

[0064]

[0065] Step 4: According to the particle set at time k+1 and the normalized weight w at time k k (i) Calculate the prior estimate at time k+1

[0066] Furthermore, in step 6, steps 3 to 5 are executed in a loop to obtain the filtering estimation value at the next moment.

[0067] The present invention has the following advantages: a quaternion particle filter attitude estimation method based on hyperhemispherical uniform deterministic sampling is proposed, called hyperhemispherical quaternion particle filter (HHQPF), which is used to estimate attitude and gyroscope deviation. The algorithm is based on the idea of ​​deterministic sampling, uses hemispherical uniform sampling on the hypersphere of unit quaternion distribution, applies the deterministic sampling scheme to quaternion filtering, and uses a small number of uniform samples and symmetric samples on the hyperhemispherical to represent the uncertainty of attitude. Compared with GA-QPF and USQUE algorithms, its accuracy is similar to that of filtering algorithms using a large number of random samples; as the number of particles increases, the computational cost of HHQPF has obvious advantages over GA-QPF. At the same time, compared with USQUE, HHQPF can also converge faster under larger initial errors. Therefore, the present invention achieves a good balance between the accuracy and computational cost of data processing, and provides a new idea for solving problems in the fields of data fusion and attitude estimation. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] The accompanying drawings are used to provide a further understanding of the present invention and constitute a part of the specification. Together with the embodiments of the present invention, they are used to explain the present invention and do not constitute a limitation of the present invention. In the accompanying drawings:

[0069] Figure 1 A framework for discrete quaternion particle filtering based on hyperhemispherical uniform deterministic sampling strategy

[0070] Figure 2 Comparison of the estimated pitch, roll, and yaw angles in the static case using HHQPF with 50 particles and GA-QPF with 500 particles and after USQUE processing;

[0071] Figure 3 Comparison of RMSE and single-step duration of USQUE, HHQPF and GA-QPF under different particle numbers using simulated data; DETAILED DESCRIPTION

[0072] The embodiments of the present invention are described in detail below. The embodiments are exemplary and are only used to explain the present invention, but cannot be understood as limiting the present invention. A quaternion particle filter attitude estimation method based on super-hemispherical uniform deterministic sampling of the present invention is described in detail below with reference to the accompanying drawings of the specification:

[0073] In order to better reflect the specific implementation steps and method effects of the present invention, the following simulation experiment was built to theoretically verify the algorithm performance of HHQPF through simulation comparison with GA-QPF and USQUE.

[0074] The parameters for this simulation verification are set as follows: Assume that the gyroscope noise parameters used by the aircraft are The initial bias was set to 0.1 degrees / hour, the bias stability of the accelerometer and magnetometer were 20 mg and 1 uT respectively, and the sampling frequency was 100 Hz for 50 s.

[0075] According to the above simulation experimental data, Figure 1 A structural block diagram of a quaternion particle filter attitude estimation method based on hyperhemispheric uniform deterministic sampling. The specific implementation steps are as follows:

[0076] Step 1: Establish a quaternion differential equation as the state equation based on the gyroscope data output, establish the system observation equation based on the output of the accelerometer and magnetometer, select the quaternion q and the gyroscope error β as the state quantity in the system, and the state equation is defined as: in yes The antisymmetric matrix of is the output value of the gyroscope, and the observation equation is defined as: in is the rotation matrix, r k is the observation vector, v k represents the Gaussian white noise of the sensor;

[0077] Step 2: Particle initialization, obtain the initial quaternion based on the initial accelerometer value g0 and the initial magnetometer value m0 Obtain the gyroscope deviation particle set based on the initial gyroscope deviation β0

[0078] Step 3: Hyperhemispheric uniform deterministic sampling, using hyperhemispheric uniform deterministic sampling to generate the particle set at time k It is used for measurement update and recursive estimation, where the process of obtaining the sample set by super-hemispheric uniform deterministic sampling is as follows:

[0079] Step 1: Define the updated estimated a priori optimal quaternion as the projection tangent point q on the manifold k ;

[0080] Step 2: Obtain a uniform sample point set on the super hemisphere through the uniform hemispherical meshing algorithm (HEQ)

[0081] Step 3: Set the sample Through nonlinear mapping (Hemispherical mapping), we get a mirror-symmetric uniform sample set in the lower hemisphere.

[0082] Step 4: Set the sample points after mirroring Project the quaternion sample set x onto the unit quaternion manifold with resolution K;

[0083] Step 5: Output in q k A deterministic sample set centered on

[0084] Step 4: Measurement update, using the particle sample set q at time k k (i) and the measured value y at time k k Calculate the particle weight at time k And normalize in is the weight of the i-th particle at time k, w k (i) is the normalized weight of the particle, and then according to the particle sample set q at time k k (i) and the normalized weight w k (i) Calculate the optimal estimate at time k Also calculates the mean of the gyroscope bias noise and variance And using the mean of the bias noise and variance Get the bias noise β k The posterior distribution of

[0085] Step 5: Time update, combining the particle sample set q at time k k (i) Biased noise posterior distribution and the gyroscope measurement at time k+1 Update the prior particle set at time k+1 and the a priori gyroscope bias noise distribution According to the particle set at k+1 time and the normalized weight w at time k k (i) Calculate the prior estimate at time k+1 According to the calculated prior estimate at time k+1

[0086] Step 6: Loop through steps 3 to 5 to get the filter value at the next moment.

[0087] Furthermore, in step 1, the quaternion is defined as q≡[q1 ρ T ] T , where T represents the matrix transpose; q1 = cos(θ / 2), is the rotation axis and θ is the rotation angle.

[0088] The angular velocity measurement sensor model used is shown below:

[0089]

[0090] in represents the continuous measured angular velocity vector, ω(t) represents the true angular velocity vector, β(t) represents the random walk noise, and ε v (t) and ε w (t) represent independent Gaussian white noise processes respectively:

[0091]

[0092] where δ(t-τ) is a Dirac mixing process, σ v and σ w Represents ε v and ε w The standard deviation of the angular velocity measurement sensor model is as follows:

[0093]

[0094] β k =β k-1 +Δtε wk

[0095] Where Δt represents the sampling interval.

[0096] Remember q k is the quaternion at time k, q k+1 is the quaternion at time k+1, then the system state equation is:

[0097]

[0098] in, is the output value of the gyroscope.

[0099] The observation equation is defined as: in is the rotation matrix, r k is the observation vector, v k represents the Gaussian white noise of the sensor, where the observation quantity r at time k kComposed of the accelerometer output and the magnetometer output, the observation equation of the system can be written as:

[0100]

[0101] in, For navigation coordinate system To the body coordinate system The posture matrix, g n is the local gravitational acceleration, m n is the local magnetic field component; v0 and m0 are the zero-mean white noise of the accelerometer and the zero-mean white noise of the magnetometer, respectively.

[0102] Furthermore, in step 2, the initial bias noise distribution of the gyroscope is obtained. The bias noise of the gyroscope obeys a zero-mean Gaussian distribution. The initial particle distribution of β0 is assumed to be: Where N(·) is a Gaussian distribution.

[0103] Furthermore, in step 3, the particle set at time k is obtained by using hyperhemispheric uniform deterministic sampling The process is as follows:

[0104] Step 1: Define the estimated optimal quaternion as the projection tangent point q on the manifold k ;

[0105] Step 2: Obtain a uniform sample point set on the super hemisphere through the uniform hemispherical meshing algorithm (HEQ)

[0106] Step 3: Sample Set Through nonlinear mapping (Hemispherical mapping), we get a mirror-symmetric uniform sample set in the lower hemisphere.

[0107] Step 4: Set the sample points after mirroring The quaternion sample set x is obtained by projecting it onto the unit quaternion manifold with resolution K, as follows:

[0108]

[0109] in, is the prior estimate, K is the projection resolution,

[0110] Step 5: Output in q k A deterministic sample set centered on

[0111] Furthermore, in Step 2 of the hyper-hemispheric uniform deterministic sampling in Step 3, a uniform sample point set on the hyper-hemispheric is obtained by the uniform hemispheric meshing algorithm (HEQ) The process is as follows:

[0112] (1) Based on and θ c =Θ(V R ) Determine the hypersphere north pole cap boundary θ c , where V R is the area of ​​the circular cap, Ω is the area of ​​the unit hyperhemisphere, N is the number of grid divisions, Θ(·) is the inverse function of the spherical area calculation function V(·);

[0113] (2) Based on Get the ideal torus angle δ1, d is the dimension, in this sampling method d = 2;

[0114] (3) Based on Get the ideal number of tori n I , and round down to get the actual number of tori n;

[0115] (4) Based on Get the ideal number of torus grids y i , and round down to get the actual number of torus grids y, where V(·) is the spherical area calculation function, θ F is an increasing sequence of torus dimensions, which can be obtained by get;

[0116] (5) According to the above process, the representation of each grid area is obtained as R = ((τ1, ..., τ d ), (υ1…,υ d )), where (τ1,…,τ d ) and (υ1…,υ d ) is a one-element array represented in polar coordinates;

[0117] (6) Use the center point of each grid area as a sample point to obtain a uniform sample point set on the super hemisphere

[0118] Furthermore, in step 4, the particle sample set q at time k is updated. k (i) and the measured value y at time k k Calculate the particle weight at time k And normalize to get w k (i) The process is as follows:

[0119]

[0120] Furthermore, in step 4, the particle sample set q at time k is updated. k (i) and the normalized weight w k (i) Calculate the optimal estimate at time k Prior Particle and the weight at time k Estimate the central quaternion at time k The central quaternion at time k Equal to the quaternion second moment P qqk The maximum eigenvalue λ max The corresponding unit normalized eigenvector is as follows:

[0121]

[0122] Where p(q k (i)|data) represents the sample q k (i) The corresponding posterior probability weight, and then calculate the posterior mean of β at the current moment and covariance as follows:

[0123]

[0124] Therefore, β k The posterior distribution of is approximately

[0125] Furthermore, in step 5, the particle set at time k+1 is updated. and bias noise distribution And calculate the prior estimate at time k+1 The process is as follows:

[0126] Step 1: From k time q k Update to get k+1 time q k+1 , and generate particle distribution as follows:

[0127]

[0128] in represents the orthogonal transfer matrix, is the angular velocity at time k.

[0129] Step 2: Predict the distribution p(q k+I|k |y 0：k ) can be expressed as:

[0130]

[0131] Where p(q k |y 0：k ) is approximately a Dirac mixture, p(q k+1 |q k )Use weighted particles approximate:

[0132]

[0133] Step 3: From gyro bias The approximate predictive distribution of is calculated as follows:

[0134]

[0135] Step 4: According to the particle set at time k+1 and the normalized weight w at time k k (i) Calculate the prior estimate at time k+1

[0136] Furthermore, in step 6, steps 3 to 5 are executed in a loop to obtain the filtering estimation value at the next moment.

[0137] In summary, the effect of this method is analyzed. Figure 2 As shown in the figure, a discrete quaternion particle filter method based on a hyperhemispherical uniform deterministic sampling strategy is proposed, referred to as the hyperhemispherical quaternion particle filter (HHQPF). Under the premise of a small number of particles (50 particles), its three-axis attitude estimation accuracy is comparable to that of the genetic algorithm embedded quaternion particle filter (GA-QPF) with 500 particles, and is significantly higher than the unscented quaternion estimator (USQUE). Figure 3 As shown in the figure, in terms of the root mean square attitude error and single-step running time under multiple simulations, the hyperhemispherical quaternion particle filter (HHQPF) has a lower computational cost while ensuring the accuracy of attitude estimation.

Claims

1. A quaternion particle filter attitude estimation method based on hyperhemispheric uniform deterministic sampling, characterized in that: The steps include: Step 1: Establish a quaternion differential equation as the state equation based on the gyroscope data output, establish the system observation equation based on the output of the accelerometer and magnetometer, select the quaternion q and the gyroscope error β as the state quantity in the system, and the state equation is defined as: in yes The antisymmetric matrix of is the output value of the gyroscope, and the observation equation is defined as: in is the rotation matrix, r k is the observation vector, v k represents the Gaussian white noise of the sensor; Step 2: Particle initialization, obtain the initial quaternion based on the initial accelerometer value g0 and the initial magnetometer value m0 Obtain the gyroscope deviation particle set based on the initial gyroscope deviation β0 Step 3: Hyperhemispheric uniform deterministic sampling, using hyperhemispheric uniform deterministic sampling to generate the particle set at time k It is used for measurement update and recursive estimation, where the process of obtaining the sample set by super-hemispheric uniform deterministic sampling is as follows: Step 1: Define the updated estimated a priori optimal quaternion as the projection tangent point q on the manifold k ; Step 2: Obtain a uniform sample point set on the super hemisphere through the uniform hemispherical meshing algorithm (HEQ) Step 3: Set the sample Through nonlinear mapping (Hemispherical mapping), we get a mirror-symmetric uniform sample set in the lower hemisphere. Step 4: Set the sample points after mirroring Project the quaternion sample set x onto the unit quaternion manifold with resolution K; Step 5: Output in q k A deterministic sample set centered on Step 4: Measurement update, using the particle sample set q at time k k (i) and the measured value y at time k k Calculate the particle weight at time k And normalize in is the weight of the i-th particle at time k, w k (i) is the normalized weight of the particle, and then according to the particle sample set q at time k k (i) and the normalized weight w k (i) Calculate the optimal estimate at time k Also calculates the mean of the gyroscope bias noise and variance And use the mean of the bias noise and variance Get the bias noise β k The posterior distribution of Step 5: Time update, combining the particle sample set q at time k k (i) Biased noise posterior distribution and the gyroscope measurement at time k+1 Update the prior particle set at time k+1 and the a priori gyroscope bias noise distribution According to the particle set at time k+1 and the normalized weight w at time k k (i) Calculate the prior estimate at time k+1 According to the calculated prior estimate at time k+1 Step 6: Loop through steps 3 to 5 to get the filter value at the next moment.

2. The quaternion particle filter attitude estimation method based on super-hemispherical uniform deterministic sampling according to claim 1 is characterized in that The uniform hemispherical meshing algorithm (HEQ) described in Step 2 in Step 3 is as follows: Step 1: According to and θ c =Θ(V R ) Determine the hypersphere north pole cap boundary θ c , where V R is the area of ​​the circular cap, Ω is the area of ​​the unit hyperhemisphere, N is the number of grid divisions, Θ(·) is the inverse function of the spherical area calculation function V(·); Step 2: According to Get the ideal torus angle δ I , d is the dimension, in this sampling method d = 2; Step 3: According to Get the ideal number of tori n I , and round down to get the actual number of tori n; Step 4: According to Get the ideal number of torus grids y i , and round down to get the actual number of torus grids y, where V(·) is the spherical area calculation function, θ F is an increasing sequence of torus dimensions, which can be obtained by get; Step 5: According to the above process, the representation of each grid area is obtained R = ((τ1, ..., τ d ), (υ1…,υ d )), where (τ1,…,τ d ) and (υ1…,υ d ) is a one-element array represented in polar coordinates; Step 6: Use the center point of each grid area as a sample point to obtain a uniform sample point set on the hyperhemisphere

Images