UAV attitude estimation (complementary filtering)

The three basic algorithms of gyro attitude estimation, such as the integral algorithm and the gradient filtering algorithm, are used to realize the attitude estimation of the gyro at the same time, It is necessary to find a sensor that does not change with time to estimate the attitude and calibrate it. The three methods have their own advantages and disadvantages. The complementary and gradient algorithm is more suitable for dealing with aircraft with limited performance, such as micro four-axis, which adopts low-frequency Cortex-M0 or M1 processor. When the conditions permit and the performance is sufficient, it is suggested to consider the algorithm of extended Kalman filter

The improved complementary filtering algorithm is adopted in the PX4 source code. On the basis of the original accelerometer calibration gyroscope, the magnetometer and GPS data are added for further quaternion calibration compensation. Its advantage is simple operation, so it has become the default attitude solution algorithm in PX4

Reference article: [basic algorithm learning 4] complementary filtering algorithm - PX4 attitude estimation

      PX4 source code analysis 3: attitude_estimator_q

      Schmidt orthogonalization

Firmware version: v1.0 eleven point three

Noun interpretation


   gyroscope, which measures angular velocity and has high dynamic characteristics, is a device for indirectly measuring angle. It measures the derivative of the angle, that is, the angular velocity. The angle can be obtained only by integrating the angular velocity with time. Due to the influence of noise and other errors, they accumulate continuously under the action of integration, which eventually leads to the low-frequency interference and drift of gyroscope.


   output the direction of current acceleration (including gravity acceleration g) [also known as gravity sensor]. When hovering, the output is g. The high-frequency signal sensitivity caused by its measurement principle makes the accelerometer have large high-frequency interference in the vibration environment.

Magnetometer (also known as magnetic compass)

  the output is the angle between the current body and the geomagnetic field. The measuring principle is similar to that of a compass. It has good low-frequency characteristics and is easy to be disturbed by the surrounding magnetic field.
Reference to the working principle of magnetometer:

Coordinate system

  • Navigation coordinate system: in multi rotor, it is also called earth coordinate system and geographic coordinate system. Generally, the North East earth (NED) is used to form the X, y and Z axes of the coordinate system.
  • Body coordinate system: fixed on the multi rotor aircraft, that is, the origin of the coordinate system is located at the center point of the aircraft (assuming that the center point coincides with the center of gravity).

For the coordinate system used in aviation navigation, please refer to AIAA series. In the multi rotor, because it only flies at low altitude and the time is short, only the above two are required.

Gesture representation

  • Euler angle: it is more intuitive and describes the attitude of rigid body in three-dimensional Euclidean space. At this time, the coordinate system is the body coordinate system, which rotates with the rotation of the rigid body. Disadvantages: universal joint lock.
  • Quaternion: a set of four-dimensional vectors representing the three-dimensional rotation of a rigid body. Suitable for computer calculation.
  • Direction cosine matrix DCM: the rotation of the coordinate system expressed by the cosine value of Euler angle or quaternion.

Filtering principle

   complementary filtering requires the interference noise of the two signals to be at different frequencies. By setting the cut-off frequency of the two filters, it is ensured that the fused signal can cover the required frequency.
   in the attitude estimation of IMU, the complementary filter uses high pass filtering for the gyroscope (low-frequency noise); Use low frequency filtering for accelerometer / magnetometer (high frequency noise).

   in complementary filtering, the cut-off frequencies of the two filters are the same. At this time, it is necessary to choose them according to the value of useful frequency.

   when the body is horizontal, the accelerometer cannot measure the rotation around the Z axis, i.e. yaw angle. The magnetometer has the same problem. It cannot measure the rotation of the magnetic shaft. Therefore, it is necessary to calibrate the gyroscope with accelerometer and magnetometer at the same time.

UAV attitude estimation part

Header file part code

//Call the header file of UAV attitude and parameters
#include <float.h>

#include <drivers/drv_ hrt. H / / high precision timer. In the control process, most links use the classical PID control algorithm
//To obtain the most important time variable of real-time response, which involves how to obtain high-precision time. pixhawk has high-precision RTC (real-time clock)
//The header file is introduced
#include <lib/ecl/geo/geo.h>
#include <lib/ecl/geo_lookup/geo_mag_declination.h>
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <matrix/math.hpp>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/posix.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/vehicle_odometry.h>

using keyword

using matrix::Dcmf;

using matrix::Eulerf;

using matrix::Quatf;

using matrix::Vector3f;

using matrix::wrap_pi;

**Using keyword: * * indicates that the name is introduced into the declaration area where the using description appears

Class definition

class AttitudeEstimatorQ : public ModuleBase<AttitudeEstimatorQ>, public ModuleParams, public px4::WorkItem

	~AttitudeEstimatorQ() override;

	/** @see ModuleBase */
	static int task_spawn(int argc, char *argv[]);

	/** @see ModuleBase */
	static int custom_command(int argc, char *argv[]);

	/** @see ModuleBase */
	static int print_usage(const char *reason = nullptr);

	bool init();//Initialization function


	void Run() override;

	void update_parameters(bool force = false);

	bool init_attq();

	bool update(float dt); //dt is the update cycle. This function is the core of the whole program

	// Update magnetic declination (in rads) immediately changing yaw rotation
	//Update the magnetic yaw angle (in rads) and immediately change the yaw rotation
	//When the yaw angle changes, the magnetic azimuth is updated immediately
	void update_mag_declination(float new_declination);

	const float _eo_max_std_dev = 100.0f;		/**< Maximum permissible standard deviation for estimated orientation */
	//Maximum allowable standard deviation of estimated azimuth

	const float _dt_min = 0.00001f;//dt refers to the time interval of updating data, that is, T in the discrete pid formula
	const float _dt_max = 0.02f;

	uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)};

	uORB::Subscription		_parameter_update_sub{ORB_ID(parameter_update)};
	uORB::Subscription		_gps_sub{ORB_ID(vehicle_gps_position)};
	uORB::Subscription		_local_position_sub{ORB_ID(vehicle_local_position)};
	uORB::Subscription		_vision_odom_sub{ORB_ID(vehicle_visual_odometry)};

	//In topicvehicle_ Inside the odometry
	uORB::Subscription		_mocap_odom_sub{ORB_ID(vehicle_mocap_odometry)};
	uORB::Subscription		_magnetometer_sub{ORB_ID(vehicle_magnetometer)};

	//UAV attitude data release
	uORB::Publication<vehicle_attitude_s>	_att_pub{ORB_ID(vehicle_attitude)};

	int		_lockstep_component{-1};

	float		_mag_decl{0.0f};/**Magnetic azimuth**/
	float		_bias_max{0.0f};/**Maximum deviation**/

	Vector3f	_gyro; //Angular velocity of three axes obtained by gyroscope
	Vector3f	_accel;  //Triaxial acceleration obtained by accelerometer
	Vector3f	_mag;    //Magnetic heading obtained by magnetometer

	Vector3f	_vision_hdg;//Visual position estimation;
	Vector3f	_mocap_hdg; //vicon attitude position estimation;

	Quatf		_q;    //Quaternion
	Vector3f	_rates;  //Attitude angular velocity and above_ Unlike gyro, this one is used to store the corrected angular velocity around the three axes
	Vector3f	_gyro_bias;  //Gyro deviation

	Vector3f	_vel_prev;  //Previous acceleration

	//Absolute time
	hrt_abstime	_vel_prev_t{0};//The absolute time when calculating the speed at the previous moment is used to calculate the acceleration according to the speed difference divided by the time difference

	Vector3f	_pos_acc;//Motion acceleration, note: this acceleration does not include the acceleration in the vertical direction

	hrt_abstime	_last_time{0};

	bool		_inited{false};//Initialization identification;
	bool		_data_good{false};  //Data availability;
	bool		_ext_hdg_good{false};//External heading available;

	//Parameter definition
	/**w in front means weight**/
		(ParamFloat<px4::params::ATT_W_ACC>) _param_att_w_acc,  //Accelerometer weight 0.2f
		(ParamFloat<px4::params::ATT_W_MAG>) _param_att_w_mag,  //Magnetometer weight 0.1f
		(ParamFloat<px4::params::ATT_W_EXT_HDG>) _param_att_w_ext_hdg,   //External heading weight 0.1f
		(ParamFloat<px4::params::ATT_W_GYRO_BIAS>) _param_att_w_gyro_bias,   //Gyro deviation weight 0.1f
		(ParamFloat<px4::params::ATT_MAG_DECL>) _param_att_mag_decl,   //Magnetic declination (°) 0.0f
		(ParamInt<px4::params::ATT_MAG_DECL_A>) _param_att_mag_decl_a, //Enable GPS based automatic declination correction 1
		(ParamInt<px4::params::ATT_EXT_HDG_M>) _param_att_ext_hdg_m,   //External heading mode 0
		(ParamInt<px4::params::ATT_ACC_COMP>) _param_att_acc_comp,   // Enable acceleration compensation based on GPS speed 1
		(ParamFloat<px4::params::ATT_BIAS_MAX>) _param_att_bias_mas,   //Upper limit of gyro deviation 0.05f
		(ParamInt<px4::params::SYS_HAS_MAG>) _param_sys_has_mag   //Vibration level alarm threshold 0.2f

Parameter definition

	//Parameter definition
    /**w in front means weight**/
		(ParamFloat<px4::params::ATT_W_ACC>) _param_att_w_acc,  //Accelerometer weight 0.2f
		(ParamFloat<px4::params::ATT_W_MAG>) _param_att_w_mag,  //Magnetometer weight 0.1f
		(ParamFloat<px4::params::ATT_W_EXT_HDG>) _param_att_w_ext_hdg,   //External heading weight 0.1f
		(ParamFloat<px4::params::ATT_W_GYRO_BIAS>) _param_att_w_gyro_bias,   //Gyro deviation weight 0.1f
		(ParamFloat<px4::params::ATT_MAG_DECL>) _param_att_mag_decl,   //Magnetic declination (°) 0.0f
		(ParamInt<px4::params::ATT_MAG_DECL_A>) _param_att_mag_decl_a, //Enable GPS based automatic declination correction 1
		(ParamInt<px4::params::ATT_EXT_HDG_M>) _param_att_ext_hdg_m,   //External heading mode 0
		(ParamInt<px4::params::ATT_ACC_COMP>) _param_att_acc_comp,   // Enable acceleration compensation based on GPS speed 1
		(ParamFloat<px4::params::ATT_BIAS_MAX>) _param_att_bias_mas,   //Upper limit of gyro deviation 0.05f
		(ParamInt<px4::params::SYS_HAS_MAG>) _param_sys_has_mag   //Vibration level alarm threshold 0.2f

Initialization assignment

//Initialization of data
AttitudeEstimatorQ::AttitudeEstimatorQ() :
	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
{;/**Clear**/;;;;;/**Clear the heading obtained by vision**/;/**The heading obtained by mocap is cleared**/;;;


	_lockstep_component = px4_lockstep_register_component();


//Destructor, clear all tasks


//Initialization function
	if (!_sensors_sub.registerCallback()) {
		PX4_ERR("sensor combined callback registration failed!");
		return false;

	return true;

Run function

	if (should_exit()) {

	sensor_combined_s sensors;

	//If the parameter is updated, enter the following function
	if (_sensors_sub.update(&sensors)) {

		//Parameter update
		//Update the quaternion according to this deflection angle

		// Feed validator with recent sensor data
		//Fill the verifier with recent data for data verification

		if (sensors.timestamp > 0) {
			_gyro(0) = sensors.gyro_rad[0];
			_gyro(1) = sensors.gyro_rad[1];
			_gyro(2) = sensors.gyro_rad[2];

		if (sensors.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
			_accel(0) = sensors.accelerometer_m_s2[0];
			_accel(1) = sensors.accelerometer_m_s2[1];
			_accel(2) = sensors.accelerometer_m_s2[2];

            //The length function is the square of the sum of squares
			if (_accel.length() < 0.01f) {
				PX4_ERR("degenerate accel!");

		// Update magnetometer
		//If the data is updated, the data is assigned
		//The only variables defined in topic are magnetometer_ga[3]
		if (_magnetometer_sub.updated()) {
			vehicle_magnetometer_s magnetometer;

			if (_magnetometer_sub.copy(&magnetometer)) {
				_mag(0) = magnetometer.magnetometer_ga[0];
				_mag(1) = magnetometer.magnetometer_ga[1];
				_mag(2) = magnetometer.magnetometer_ga[2];

				if (_mag.length() < 0.01f) {
					PX4_ERR("degenerate mag!");


		_data_good = true;

		// Update vision and motion capture heading
		//Update visual and motion capture titles
		_ext_hdg_good = false;

		if (_vision_odom_sub.updated()) {
			vehicle_odometry_s vision;

			if (_vision_odom_sub.copy(&vision)) {
				// validation check for vision attitude data
				//Verification and inspection of visual attitude data

				// vision_att_valid is a bool type, which is mainly used to judge, but what does the data in it mean?
				bool vision_att_valid = PX4_ISFINITE(vision.q[0])
							&& (PX4_ISFINITE(vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf(
											vision.pose_covariance[vision.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true);

				//If effective
				if (vision_att_valid) {

					//Assign the quaternion based on vision to the new math::q

					//The transformation matrix is obtained, and the visual quaternion is transformed into the transformation matrix
					Dcmf Rvis = Quatf(vision.q);
					//This is a North pointing vector. What is 0.4f?
					Vector3f v(1.0f, 0.0f, 0.4f);

					// Rvis is Rwr (robot respect to world) while v is respect to world.
					// Rvis is Rwr (robot's relationship to the world), and v is the relationship to the world.
					//v is based on the world coordinates, and dcm is transposed by B -- > E

					// Hence Rvis must be transposed having (Rwr)' * Vw
					// Therefore, Rvis must be transposed with (RWR) '* VW
					// Rrw * Vw = vn. This way we have consistency
					// Rrw * Vw = vn.  We have consistency

					//The meaning of "transfer"
					_vision_hdg = Rvis.transpose() * v;/**After transposing and multiplying with v, it reflects the visual vector pointing north**/

					// vision external heading usage (ATT_EXT_HDG_M 1)
					// Visual external Title Usage
					if (_param_att_ext_hdg_m.get() == 1) {
						// Check for timeouts on data
						// Check data timeout
						_ext_hdg_good = vision.timestamp_sample > 0 && (hrt_elapsed_time(&vision.timestamp_sample) < 500000);

		if (_mocap_odom_sub.updated()) {
			vehicle_odometry_s mocap;

			if (_mocap_odom_sub.copy(&mocap)) {
				// validation check for mocap attitude data
				bool mocap_att_valid = PX4_ISFINITE(mocap.q[0])
						       && (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf(
										       mocap.pose_covariance[mocap.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true);

				if (mocap_att_valid) {
					Dcmf Rmoc = Quatf(mocap.q);
					Vector3f v(1.0f, 0.0f, 0.4f);

					// Rmoc is Rwr (robot respect to world) while v is respect to world.
					// Hence Rmoc must be transposed having (Rwr)' * Vw
					// Rrw * Vw = vn. This way we have consistency
					_mocap_hdg = Rmoc.transpose() * v;

					// Motion Capture external heading usage (ATT_EXT_HDG_M 2)
					if (_param_att_ext_hdg_m.get() == 2) {
						// Check for timeouts on data
						_ext_hdg_good = mocap.timestamp_sample > 0 && (hrt_elapsed_time(&mocap.timestamp_sample) < 500000);

		if (_gps_sub.updated()) {
			vehicle_gps_position_s gps;

			if (_gps_sub.copy(&gps)) {
				//First, check whether automatic magnetic azimuth acquisition is configured, that is, use the following if
				if (_param_att_mag_decl_a.get() && (gps.eph < 20.0f)) {
					/* set magnetic declination automatically */
					// Automatically set magnetic declination
					//Automatically obtain the magnetic azimuth. If configured, use the current longitude and latitude
					// Through get_ mag_ The declination (_gpos. Lat, _gpos. Lon) function obtains the magnetic declination of the current position
					update_mag_declination(get_mag_declination_radians(, gps.lon));
				//If configured, the current magnetic declination is used

		// Local location update
		if (_local_position_sub.updated()) {
			vehicle_local_position_s lpos;

			if (_local_position_sub.copy(&lpos)) {

				//Obtain the body acceleration and calculate the body acceleration by calculating the speed
				//First judge whether the location information is valid
				if (_param_att_acc_comp.get() && (hrt_elapsed_time(&lpos.timestamp) < 20_ms)
				    && lpos.v_xy_valid && lpos.v_z_valid && (lpos.eph < 5.0f) && _inited) {

					/* position data is actual */
					const Vector3f vel(lpos.vx, lpos.vy, lpos.vz);

					/* velocity updated */
					if (_vel_prev_t != 0 && lpos.timestamp != _vel_prev_t) {
						float vel_dt = (lpos.timestamp - _vel_prev_t) / 1e6f;
						/* calculate acceleration in body frame */
						// Acceleration of computer body
						// Calculate the acceleration of motion in e system, here pos_acc is very important and should be used for the calibration of the rear accelerometer
						_pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt);

					_vel_prev_t = lpos.timestamp;//Prepare for iteration

					//The last time is to assign the updated data to a variable
					_vel_prev = vel;

				} else {
					/**Otherwise, the position information is outdated and the acceleration information is cleared**/
					/* position data is outdated, reset acceleration */
					// Reset the data without updating_ vel_ prev_ When the initial value of this variable is 0, the update will replace the data with LPOS Timestamp, which is assigned to 0 without updating;;
					_vel_prev_t = 0;

		/* time from previous iteration */
		// Time since last iteration
		hrt_abstime now = hrt_absolute_time();
		//Conditional expression from right to left
		//The following constrain t function is used to limit the action. The qualification rule is: return (Val < min_val)? min_ val : ((val > max_val) ?  max_ val : val);
		const float dt = math::constrain((now - _last_time) / 1e6f, _dt_min, _dt_max);
		_last_time = now;

		if (update(dt)) {
			vehicle_attitude_s att = {};
			att.timestamp = sensors.timestamp;

			/* the instance count is not used here */
			// Instance counting is not used here
			//Finally, it was released to vehicle_attitude, which corresponds to what we said at the beginning. We can't directly regard the data of gyroscope as attitude information, but
            		//It can only be used after our treatment. Vehicle_ The information flow of attitude is: sensor combined is subscribed and vehicle attitude is published


Parameter update

//Check whether the parameter is updated, false check whether the parameter is updated, true get the parameter, and update the quaternion by the magnetic declination
AttitudeEstimatorQ::update_parameters(bool force)
	// check for parameter updates
	if (_parameter_update_sub.updated() || force) {
		// clear update

		/**Create a construct pupdate, which has the update time, whether to update and fill in information**/
		parameter_update_s pupdate;

		// update parameters from storage
		// Update parameters from storage

		// disable mag fusion if the system does not have a mag
		// If the system does not have blocks, block fusion is disabled
		if (_param_sys_has_mag.get() == 0) {

		// if the weight is zero (=mag disabled), make sure the estimator initializes
		// If the weight is zero (= mag disabled), make sure the estimator is initialized
		if (_param_att_w_mag.get() < FLT_EPSILON) {
			_mag(0) = 1.f;
			_mag(1) = 0.f;
			_mag(2) = 0.f;

		// math::radians converts angles to radians

Vector computing tips

nameStandard product / inner product / quantity product / dot productVector product / outer product / vector product / cross product
Operation formulaa·b=|a||b|·cosθa × b=c, where | c | = | a | B | sin θ, The direction of c follows the right-hand rule
Geometric meaningThe product of the projection of vector a in the direction of vector b and the module of vector bc is the plane perpendicular to a and b, and expressed as | b · sin θ Is the area of a parallelogram with height and | a | as the bottom
Difference of operation resultsScalar (often used in Physics) / quantity (often used in Mathematics)Vector (often used in Physics) / vector (often used in Mathematics)

**Vector normalization: * * one is to change the number between (0, 1) decimal One is to change the dimensional expression into Dimensionless expression. Mainly for data processing It's convenient to bring it up. Put it data Mapping to the range of 0 ~ 1 is more convenient and fast.

eg: {2.5 3.5 0.5 1.5} normalized to {0.3125 0.4375 0.0625 0.1875}

Solution: 2.5 + 3.5 + 0.5 + 1.5 = 8,





This normalization is to change the sum in parentheses into 1, and then write the proportion of each number.

Initialize attitude estimation

/***********init_attq() Function function: the rotation matrix and quaternion are initialized by accelerometer and magnetometer**************/
	// Rotation matrix can be easily constructed from acceleration and mag field vectors
	// The rotation matrix can be easily constructed from acceleration and magnetic field vectors
	// 'k' is Earth Z axis (Down) unit vector in body frame
	// 'k' is the unit vector of the earth's Z-axis (Down) in the volume frame

	//The triaxial acceleration k obtained by the acceleration sensor is the direction vector of acceleration. When the UAV is stable, that is, there is no yaw
	Vector3f k = -_accel;
				      //_ accel has been subscribed in Run(), and is assigned
                                      //k is the acceleration direction vector measured by the acceleration sensor. Since the UAV is generally in a steady state and no motion state when measuring the data for the first time,
                                      //Therefore, the measured acceleration can be directly regarded as the gravitational acceleration g, which is taken as the third row k of dcm rotation matrix

	//k is a downward vector
	k.normalize();//Vector normalization is the vector divided by its length

	// 'i' is Earth X axis (North) unit vector in body frame, orthogonal with 'k'
	// 'i' is the unit vector of the earth's X-axis (North) in the volume frame, which is orthogonal to 'k'.
	Vector3f i = (_mag - k * (_mag * k));
    					//Schmidt orthogonalization is explained in the preface
						//Schmidt orthogonalization, forcing i to be perpendicular to K; Since the vector k has been normalized, the denominator is equal to 1;
                                          	//Here_ mag refers to the north, so the R obtained below is the default, and the aircraft nose also refers to the north

	//i is the vector facing north
	i.normalize();				//Vector normalization is the vector divided by its length

	// 'j' is Earth Y axis (East) unit vector in body frame, orthogonal with 'k' and 'i'
	// 'j' is the unit vector of the earth's Y axis (East) in the e coordinate, orthogonal to 'k' and 'i'.

	//j is the vector facing east
	Vector3f j = k % i;

	// Fill rotation matrix
	Dcmf R;   //3 X 3 rotation matrix
	R.row(0) = i;//North fills the i vector in the first row of the matrix
	R.row(1) = j;//Fill the j vector in the second row of the matrix
	R.row(2) = k;//Next, fill the k vector into the third row of the matrix. Note: from here, we can see that the rotation matrix is b-system to n-system

	// Convert to quaternion
	// Convert R matrix to Quaternion
	_q = R;

	// Compensate for magnetic declination compensates the magnetic azimuth of the aircraft, because the previous q is the default aircraft pointing north, but the aircraft does not necessarily point north when taking off
	Quatf decl_rotation = Eulerf(0.0f, 0.0f, _mag_decl);   /**The rotation matrix is expressed only by the quaternion of yaw**/
	_q = _q * decl_rotation;

	_q.normalize();  /**Normalization**/  /**q at this point is the real initial q**/

	if (PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&    /**Judge whether it diverges**/
	    PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) &&
	    _q.length() > 0.95f && _q.length() < 1.05f) {
		_inited = true;     /**Initialization complete**/

	} else {
		_inited = false;   /**Initialization failed**/

	return _inited;

Update and update quaternion

//Initialization occurs only once
AttitudeEstimatorQ::update(float dt)
	if (!_inited) {

		if (!_data_good) {  /**In the previous Run() function, if the sensor data subscription is normal, the data_good is true**/
			return false;

		return init_attq();
				/**Initialize without initialization (premise: sensor data subscription is normal)**/
                        	//Note that init is executed only once in a flight trampoline,
				// After the aircraft passes through the Kutta method, the output angle of the aircraft needs to be calculated by init_ Finding transformation matrix by attq


	/**Previous init_ When attq is completed, continue to execute**/
	Quatf q_last = _q;  /**Note: quaternion_ q already has content. Here's q_ The function of last is to get a backup for q, which can be seen from the last if**/

	// Angular rate of correction
	Vector3f corr;

	//_ gyro is the result of the assignment of Run() above
	float spinRate = _gyro.length();   /**Define a variable: rotation rate, and the length function is the square of the sum of squares**/

	//First, judge what mode is used for correction, such as vision, mocap, acc and mag. here we focus on the analysis of using acc and MAG for correction, and the other two are the same
	if (_param_att_ext_hdg_m.get() > 0 && _ext_hdg_good) {
		if (_param_att_ext_hdg_m.get() == 1) {
			// Vision heading correction modifies the vision based heading mode
			// Project heading to global frame and extract XY component
			Vector3f vision_hdg_earth = _q.conjugate(_vision_hdg); /**Convert b system to e system**/
			float vision_hdg_err = wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0)));
			// Project correction to body frame
			corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -vision_hdg_err)) * _param_att_w_ext_hdg.get();

		if (_param_att_ext_hdg_m.get() == 2) {
			// Mocap heading correction
			// Project heading to global frame and extract XY component
			Vector3f mocap_hdg_earth = _q.conjugate(_mocap_hdg);
			float mocap_hdg_err = wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0)));
			// Project correction to body frame
			corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -mocap_hdg_err)) * _param_att_w_ext_hdg.get();

	//Magnetometer calibration external navigation failure or not in use
    //Convert the magnetometer reading from the body coordinate system to the navigation coordinate system
    if (_param_att_ext_hdg_m.get() == 0 || !_ext_hdg_good) {
		// Magnetometer correction
		// Project mag field vector to global frame and extract XY component
		// Project the mag field vector to the global frame and extract the XY component
		Vector3f mag_earth = _q.conjugate(_mag);  /**Convert b system to e system**/   //The formula is shown in figure-1

		//Only vector < 3 > MAG is considered_ Data MAG of the first two dimensions in earth_ Earth (1) and mag_earth(0) (i.e. x, y, ignoring the offset on the z axis), through
       		//arctan (mag_earth (1), MAG_ The angle obtained by Earth (0)) is different from the magnetic azimuth obtained by longitude and latitude to obtain the error angle mag_err
      		//wrap_pi function is used to limit the result from - pi to PI. If it is greater than PI, it will make up with 2pi, and if it is less than - pi, it will make up with 2pi
      		//Note 1: here, the correction method of gps calibration is used to correct the magnetic timing, that is, subtract the automatically obtained magnetic declination from the following. No gps calibration method (see my ppt)
     		//Note 2: the correction method here is to use the difference between the magnetic declination calculated by the magnetometer and that obtained by gps, and then convert it to b system.

		float mag_err = wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
        //atan2f: 2 indicates two input parameters; Support angle conversion within four quadrants; Output radian value; 
        //Convert the reading of magnetometer from vector to angle; The magnetic deviation is obtained by subtracting the magnetic deviation from the angle; 
		//_ mag_decl is obtained by GPS; 
		float gainMult = 1.0f;
		const float fifty_dps = 0.873f;

		//float spinRate = _gyro.length();   /** Define a variable: rotation rate, and the length function is the square of the sum of squares**/
		if (spinRate > fifty_dps) {
			gainMult = math::min(spinRate / fifty_dps, 10.0f);

		// Project magnetometer correction to body frame
		//The following * vector < 3 > (0.0F, 0.0F, - mag_err)) is the level at which raw should essentially be generated by the magnetometer
		// The magnetic vector is obtained by cross multiplication with the horizontal magnetic vector generated by gps, and the cross multiplication is just down
        //Convert the magnetic field deviation to the body coordinate system and multiply it by its corresponding weight; 
        //Figure 2
		corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -mag_err)) * _param_att_w_mag.get() * gainMult;

	_q.normalize();//The normalization here is used to correct the update_ mag_ Deviation after declination.

	// Accelerometer correction
	// Project 'k' unit vector of earth frame to body frame
	// Vector3f k = _q.conjugate_inversed(Vector3f(0.0f, 0.0f, 1.0f));   // From n-series to b-series
	// Optimized version with dropped zeros
	//The following k is obtained by converting the unit vector of gravity in n system into b system, that is, left multiplying the rotation matrix
	Vector3f k(
		2.0f * (_q(1) * _q(3) - _q(0) * _q(2)),
		2.0f * (_q(2) * _q(3) + _q(0) * _q(1)),
		(_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3))

	//The direction of g is obtained by subtracting the direction of body acceleration (_pos_acc) from the direction of total resultant force (_accel), that is, the gravitational acceleration is obtained by subtracting the body motion acceleration from the total acceleration (acceleration acquisition)
   	//The direction of gravity acceleration is the z axis under the navigation coordinate system. After adding the motion acceleration, the direction of the total acceleration is not parallel to the sky or ground of the navigation coordinate system, so this error should be eliminated, that is "_accel-_pos_acc"
    	//Here, multiplied by the difference k, we get the angle sinx with the direction of gravity, which is about equal to x, that is, roll and pitch

	// If we are not using acceleration compensation based on GPS velocity,
	// fuse accel data only if its norm is close to 1 g (reduces drift).   Only when its norm is close to 1 g (reducing drift), acel data can be fused.
	const float accel_norm_sq = _accel.norm_squared();
	const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f;
	const float lower_accel_limit = CONSTANTS_ONE_G * 0.9f;

	if (_param_att_acc_comp.get() || ((accel_norm_sq > lower_accel_limit * lower_accel_limit) &&
					  (accel_norm_sq < upper_accel_limit * upper_accel_limit))) {

		// %It means cross multiplication
        //Vector k is the gravitational acceleration vector (0,0,1) converted to the body coordinate system; 
        //_ accel is the reading of accelerometer; 
        //_ pos_acc is the acceleration derived from the differential of position estimation (GPS); 
        //_ accel - _pos_acc represents the acceleration vector of the aircraft minus its horizontal component; 
		//Cross multiply k with (_accel - _pos_acc) to obtain the deviation;
        //See figure-3 for the formula
        //Here is another understanding
        //The direction of g is obtained by subtracting the body acceleration direction (_pos_acc) from the total resultant force direction (_accel), that is, the total acceleration (acceleration acquisition) minus the body motion acceleration to obtain the gravity acceleration, and then the attitude matrix is either row or column to cross product with the pure gravity acceleration to calculate the error. Because motion acceleration is harmful interference, it must be reduced. The theoretical basis of the algorithm is the multiplication of [0,0,1] and attitude matrix.
        //The direction of the gravitational acceleration obtained by the difference is the Z axis under the navigation coordinate system. After adding the motion acceleration, the direction of the total acceleration is not parallel to the sky or ground of the navigation coordinate system, so this error should be eliminated, that is "_accel-_pos_acc". Then cross multiply the z-axis vector to get the error and calibrate it.
		corr += (k % (_accel - _pos_acc).normalized()) * _param_att_w_acc.get();

	// Gyro bias estimation
	//spinRate rotation rate
	if (spinRate < 0.175f) {
        //Get the gyro correction value 
        //Here, the correction value is the integral part of the pi controller of mahony filter;
        //Because_ gyro_bias does not return to zero, so it continues to accumulate; 
        //The formula is shown in figure-4
		_gyro_bias += corr * (_param_att_w_gyro_bias.get() * dt);  //Corresponding integral control

		//Right_ gyro_bias does constraint processing.
		for (int i = 0; i < 3; i++) {
			_gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);


	_rates = _gyro + _gyro_bias; //Angular velocity = measured value of gyroscope + error calibration Figure 5

	// Feed forward gyro

	// Embodiment of pi controller, corresponding to proportional part
	corr += _rates;   //Corr is a variable defined in the update function, so the data of the corr variable will be refreshed every time you enter the update() function
	//Here, corr = (Ea+Em) + (Ea+Em)*dt + gyro; 
	//Personally, I think the corr here is the updated angular velocity;
    //See the formula in Figure 6
	// Apply correction to state
	//Finally, update the quaternion with the corrected data and_ rates and_ gyro_bias is set to zero for the next call.
    _q += _q.derivative1(corr) * dt;//Updating quaternions by Runge Kutta method
    //Update the quaternion, derivative is the derivative function, and use the first-order Runge Kutta derivative. See figure-6 for the formula

	// Normalize quaternion

	//If the quaternion is not appropriate, reset it
	if (!(PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
	      PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) {
		// If the data is not suitable, restore the q of the backup, that is, q_last

		// Reset quaternion to last good state
		// Resets the quaternion to the last good state
		_q = q_last;;;
		return false;

	return true;

//After updating the quaternion, we jump to the place where the update function is called, that is, line 422. We find that the updated attitude information has been released later. This is the end of the whole process
// The calling function is
// if (update(dt)) {


The above formulas are as follows:

Update yaw angle

/**When the yaw angle changes, the magnetic azimuth is updated immediately**/
AttitudeEstimatorQ::update_mag_declination(float new_declination)
	// Apply initial declination or trivial rotations without changing estimation
	// Ignore small rotation (such as small vibration of the body) and continue to use the previous magnetic azimuth
	if (!_inited || fabsf(new_declination - _mag_decl) < 0.0001f) {
		_mag_decl = new_declination;

	} else {
		//When the rotation is large, correct the attitude (q) and update the magnetic azimuth
		// Immediately rotate current estimation to avoid gyro bias growth
		Quatf decl_rotation = Eulerf(0.0f, 0.0f, new_declination - _mag_decl);
		// The quaternion generated by yaw angle is generated by making the other two angles 0 and using the Euler angle to quaternion formula
		_q = _q * decl_rotation;
		//Here, the multiplication of two quaternions indicates the addition of angles. Since q1*q2 represents the synthesis action of q1q2 mathematically, quaternions are corrected here
		_mag_decl = new_declination;
		//Use new magnetic declination

Some configuration functions

AttitudeEstimatorQ::custom_command(int argc, char *argv[])
	return print_usage("unknown command");

AttitudeEstimatorQ::task_spawn(int argc, char *argv[])
	AttitudeEstimatorQ *instance = new AttitudeEstimatorQ();

	if (instance) {;
		_task_id = task_id_is_work_queue;

		if (instance->init()) {
			return PX4_OK;

	} else {
		PX4_ERR("alloc failed");

	delete instance;;
	_task_id = -1;

	return PX4_ERROR;

AttitudeEstimatorQ::print_usage(const char *reason)
	if (reason) {
		PX4_WARN("%s\n", reason);

### Description
Attitude estimator q.


	PRINT_MODULE_USAGE_NAME("AttitudeEstimatorQ", "estimator");

	return 0;

extern "C" __EXPORT int attitude_estimator_q_main(int argc, char *argv[])
	return AttitudeEstimatorQ::main(argc, argv);

Follow up to add

Added by stargate03 on Mon, 07 Mar 2022 15:57:06 +0200