Dynamic residual threshold adaptive quaternion particle filter attitude calculation data fusion method

Abstract

The invention provides a dynamic residual threshold adaptive quaternion particle filter attitude calculation data fusion method, which belongs to the technical field of digital filtering and multi-sensor data fusion, and is mainly used for improving the precision of carrier attitude estimation. According to the method, a standard particle filter algorithm is used as a framework, data of a gyroscope, an accelerometer and a magnetometer are fused, a relatively accurate sensor measurement model is established by analyzing sensor errors, quaternion is selected as an attitude parameter, a reasonable range of residual errors is predicted and judged, then a measurement noise matrix of a system is adaptively adjusted, and data fusion of sensors is realized. 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

technical field [0001] The invention provides a dynamic residual threshold adaptive quaternion particle filter attitude calculation data fusion method, which belongs to the technical field of digital filtering and multi-sensor data fusion. The method provided by the invention is suitable for nonlinear attitude measurement systems. Background technique [0002] The attitude measurement system of the carrier is actually a nonlinear system. In order to achieve accurate calculation of the attitude, it is necessary to use nonlinear filtering to fuse the data of the gyroscope, accelerometer, and magnetometer. Gravitational acceleration and magnetic field interference cause the output data to contain more noise. Both process noise and measurement noise are unknown and have time-varying characteristics. Wrong parameter estimation may lead to filtering divergence, so a suitable filtering algorithm needs to be used. Therefore, a dynamic residual threshold adaptive quaternion particle ...

AI Technical Summary

Problems solved by technology

[0003] During the actual movement of the carrier, the accelerometer and magnetometer are easily affected by non-gravity acceleration and magnetometer, resulting in a large error in the output data, thereby affecting the filtering effect. Therefore, the present invention designs a dynamic residual The threshold adaptive quaternion particle filter attitude data fusion method uses quaternion as the attitude parameter, uses the quaternion kinematics differential equation to construct the state equation of the system, and uses the output model of the accelerometer and gyroscope to establish the measurement equation of the system. Predict the dynamic residual threshold range of the system. If the residual error exceeds the range, adaptively estimate the system measurement noise matrix, and use the measurement noise matrix obtained by adaptive estimation to perform measurement update and state adaptive estimation

Images