Two-stage separation fusion attitude and heading estimation method

Abstract

The invention discloses a two-stage separation fusion attitude and heading estimation method, which comprises the following steps: establishing a Kalman filter state equation and a measuring equation of a system, wherein xk and yk denote a state vector and an observation vector of the system respectively, wk and vk are state equation process noise and observation equation measurement noise respectively, k denotes the timing of the calculation; f is the transfer function between state vectors at adjacent moments; calculating the state vector obtained from the Kalman filter state equation, and obtaining a pitch angle theta and a roll angle phi by calculating according to the figure. The two-stage separation calculation is carried out for an attitude angle and a heading angle, the use in a non-linear filtering method such as extended Kalman filter and unscented Kalman filter methods can be avoided by the established linear filter equation, and the calculation complexity of the system can be greatly reduced. Meanwhile, the first-stage Kalman filter is responsible for calculating the attitude angle using a gyroscope and an accelerometer, and a magnetic sensor does not participate in the first-stage filtering, so that the effect of the interference with the magnetic sensor on the attitude angle estimation is eliminated.

Description

technical field [0001] The invention belongs to the technical field of inertial navigation, and more specifically relates to a two-stage separation and fusion attitude and heading estimation method. Background technique [0002] In many fields such as aerospace, vehicle navigation, robot and platform stability control, heading and attitude are the key parameters to realize the control of moving objects. For aircraft, without accurate real-time heading and attitude information, the aircraft will lose control and cannot reach the destination smoothly and safely; for satellites, without accurate real-time attitude information, the solar panels cannot be opened according to the program, seriously affecting the satellite. For robots, it is necessary to obtain their own posture information in time and feed it back to the "brain" (robot control program), and then perform tasks according to the program, such as overcoming obstacles or making specific actions. Attitude and heading r...

AI Technical Summary

Problems solved by technology

Its purpose is to reduce the computational complexity and time consumption of the system by constructing a linear Kalman filter equation, thereby solving the technical problems of high computational complexity and large computational time consumption in existing estimation methods

Images