Quadcopter: Contrast of Kalman Filter with Mahony Filter

Introduction

Attitude algorithm to quadcopter is like eyes to animals. When I was study quadcopter, I find the Mahony filter is widely used as quadcopter’s attitude’s algorithm, such as PX4 and my open-source quadcopter, Minifly. While the Kalman filter has long been regarded as the optimal solution to many tracking and data prediction tasks, such as GPS navigation. I can’t stop wondering why the Kalman filter is not as popular as the Mahony filter used in attitude algorithm, regarding the fact that the Kalman filter is an effective tool to acquire true attitude. Thus, I decided to find out the feature of the Kalman filter and the Mahony filter, and find out which one is more suitable to be attitude algorithm.

The Kalman Filter

Introduction tothe Kalman Filter

The Kalman-filtering theory is such an estimating theory that yields the educated guess by combing uncertain measurement and uncertain control system models. In reality, measurement yield by sensors contain drift error, while control system models are not accurate enough to represents real world. Kalman-filtering theory assumes both measurement’s noise and model’s noise are Gaussian, then the intersection of these two Gaussian populations is very likely to represents the true state.

It’s easiest to look at this first in one dimension. A 1D Gaussian bell curve with variance σ2 and mean μ is defined as:

We want to know what happens when you multiply two Gaussian curves together. The blue curve below represents the (unnormalized) intersection of the two Gaussian populations:

Why we multiplying two Gaussian curves? The product of the PDFs of two random variables X and Y will give the joint distribution of the vector-valued random variable X and Y in the case that X and Y are independent. In the graph above, although curve 0 and curve 1 both are showing the probability distribution of variable x, curve 0’s x is obtained by measurement while curve 1’s x is obtained by control system model estimation. Thus, curve 0 and curve 1 can be regarded as independent. In other words, the overlapped part represents the probability distribution of the situation which can be measured by sensors and can be estimated by control system model simultaneously. Thus, the true state must lie in this overlapped area.

Now we will find the equation of this overlapped distribution:

The PDF of this overlapped Gaussian distribution is:

The derivation process: http://www.tina-vision.net/docs/memos/2003-003.pdf

We can simplify by factoring out a littlepiece and calling it k, which we will find is exactly equal to Kalman Gain later :

When we look at the five core Kalman filter equations, we can find that the equation above derived from PDFs’ multiplication is actually the origin of the Kalman filter:

Combining theKalman filter and attitude algorithm

Now let’s focus on how to add the Kalman filter theory in attitude algorithm. In Minifly open source quadcopter, an MPU9250 nine-axis MEMS chip is installed, and we can convert three-axis accelerometer’s reading to Euler angle, which can be regarded as measurement. The three-axis accelerometer’s model is:

For control system model of quadcopter, readers can refer to following picture:

Besides the forces exerted on the quadcopter airframe body produced by four blades, we also need to know quadcopter’s moment of inertia, and the distance between four motors and quadcopter’s mass center. Then we can calculate the angular acceleration, we can get the angular velocity after one integration with respect to time, and get the Euler angle of quadcopter after another integration. This Euler angle can be regarded as the control system model we need.

It can be found that building a control system is very complicated and some parameters are hard to obtain, while its variance is also hard to estimate. However, three-axis gyroscope can acquire angular velocity of quadcopter directly, and the Euler angle of quadcopter can be found after integrating angular velocity with respect to time. We can regard this Euler angle as control system model. The three-axis gyroscope’s model is:

EquationsandCode

The Kalman Filter contains three steps:

After checking the MPU9250 manual, the variance of the three-axis gyroscope Q and three-axis accelerometer R is found to be 0.01 and 0.000064, respectively.

In this step state parameter X and uncertainty parameter P need to be initialized. Before the quadcopter taking off, it needs to be placed on a horizontal plane. Thus, the initial pitch and roll are set to be 0. And the initial P is set to be equal to control system model variance Q.

angle->X_now=0;
angle->P_now=angle->Q_noise;
  
/*prediction*/
angle->X_predict=angle->X_now+dt*rate;
angle->P_predict=angle->P_now+angle->Q_noise;

The conversion from three-axis accelerometer’s reading to Euler angle is known as following:

y_pitch= -asinf(-acc.x)*RAD2DEG;
y_roll=atan(acc.y/acc.z)*RAD2DEG;

Note that yaw cannot be derived from three-axis accelerometer, so I will just integrate yaw’s angular velocity with respect to time, which can be read from three-axis gyroscope.

state->attitude.yaw += gyro.z * dt;

angle->KalmanGain = angle->P_predict / (angle->P_predict+angle->R_noise);
angle->X_now = 
angle->X_predict+angle->KalmanGain * (measurement-angle->X_predict);
  
angle->P_now = (1-angle->KalmanGain) * angle->P_predict;

angle->X_predict = angle->X_now+dt*rate;
angle->P_predict = angle->P_now+angle->Q_noise;

Thewholeattitudealgorithm:

void imuUpdate(Axis3f acc, Axis3f gyro, state_t *state , float dt)
{
	float y_pitch;
	float y_roll;
  
	float normalise;
	float accBuf[3] = {0.f};
	Axis3f tempacc = acc;
  
	float ptemp=0;
	float rtemp=0;
	float ytemp=0;
  
/*control system model's variance and measurement variance*/
	pitch_KF.Q_noise=0.01;
	pitch_KF.R_noise=0.000064;
	roll_KF.Q_noise=0.01;
	roll_KF.R_noise=0.000064;
  
/*accelerometer need to be normalized*/
	normalise = invSqrt(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
	acc.x *= normalise;
	acc.y *= normalise;
	acc.z *= normalise;
  
	if((acc.x != 0.0f) || (acc.y != 0.0f) || (acc.z != 0.0f))
/*if accelerometer works normally*/
	{
		y_pitch= -asinf(-acc.x)*RAD2DEG;//get measurement
		KalmanFilter(&pitch_KF, y_pitch, gyro.y, dt);
		state->attitude.pitch = -pitch_KF.X_now;
  
		y_roll=atan(acc.y/acc.z)*RAD2DEG;//get measurement
		KalmanFilter(&roll_KF, y_roll, gyro.x, dt);
		state->attitude.roll = roll_KF.X_now;
	}
	else{
/*accelerometer cannot work normally*/
		state->attitude.pitch += gyro.y * dt;
		state->attitude.roll += gyro.x * dt;
	}
  
/*calculate yaw*/
	state->attitude.yaw += gyro.z * dt;
  
	ptemp=state->attitude.pitch*DEG2RAD;
	rtemp=state->attitude.roll*DEG2RAD;
	ytemp=state->attitude.yaw*DEG2RAD;
  
/*calculate Quaternion, it is used to calculate rotation matrix which will be called 
in other part*/
	q0=cosf(rtemp*0.5)*cosf(ptemp*0.5)*cosf(ytemp*0.5)
           +sinf(rtemp*0.5)*sinf(ptemp*0.5)*sinf(ytemp*0.5);
  
	q1=-(sinf(rtemp*0.5)*cosf(ptemp*0.5)*cosf(ytemp*0.5)
           -cosf(rtemp*0.5)*sinf(ptemp*0.5)*sinf(ytemp*0.5));
  
	q2=-(cosf(rtemp*0.5)*sinf(ptemp*0.5)*cosf(ytemp*0.5)
           +sinf(rtemp*0.5)*cosf(ptemp*0.5)*sinf(ytemp*0.5));
  
	q3=-(cosf(rtemp*0.5)*cosf(ptemp*0.5)*sinf(ytemp*0.5)
           -sinf(rtemp*0.5)*sinf(ptemp*0.5)*cosf(ytemp*0.5));
  
	normalise = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
	q0 *= normalise;
	q1 *= normalise;
	q2 *= normalise;
	q3 *= normalise;
  
	imuComputeRotationMatrix();
  
	if (!isGravityCalibrated) //calibrate sensors
	{		
  
	accBuf[2] = tempacc.x* rMat[2][0] + tempacc.y * rMat[2][1] 
                    + tempacc.z * rMat[2][2];	/*accz*/
  
		calBaseAcc(accBuf);				
	}
}
  
void KalmanFilter(KF_parameter *angle, float measurement, float rate, float dt){
  
	if (KF_initialized!=true){ //initialize Kalman Filter
  
		KF_initialized=true;
  
/*calculate initial X and P*/
		angle->X_now=0;
		angle->P_now=angle->Q_noise;
  
/*predict*/
		angle->X_predict=angle->X_now+dt*rate;
		angle->P_predict=angle->P_now+angle->Q_noise;
	}
	else{
		angle->KalmanGain = 
                angle->P_predict / (angle->P_predict+angle->R_noise);
  
/*this X_predict is not updated yet, so it is actually X predicted on time k-1*/
		angle->X_now = 
                angle->X_predict+angle->KalmanGain * (measurement-angle->X_predict);
  
		angle->P_now = (1-angle->KalmanGain) * angle->P_predict;
  
/*predict*/
		angle->X_predict = angle->X_now+dt*rate;
		angle->P_predict = angle->P_now+angle->Q_noise;
	}
}
  

The Mahony Filter

The diagram shown below is known as the Mahony filter. The Mahony filter used in autopilot’s program tends to make quadcopter’s attitude, which is calculated from gyroscope’s reading, approaches attitude calculated from accelerometer’s reading through a PI controller.

The first step is calculating quaternion through Runge-Kutta method. The gyroscope’s reading is exactly the differentiation of quadcopter’s attitude; thus, they are used to update ordinary differentiate equations.

/*update quaternion through Runge–Kutta methods in discrete system*/
	float q0Last = q0;
	float q1Last = q1;
	float q2Last = q2;
	float q3Last = q3;
	q0 += (-q1Last * gyro.x - q2Last * gyro.y - q3Last * gyro.z) * halfT;
	q1 += ( q0Last * gyro.x + q2Last * gyro.z - q3Last * gyro.y) * halfT;
	q2 += ( q0Last * gyro.y - q1Last * gyro.z + q3Last * gyro.x) * halfT;
	q3 += ( q0Last * gyro.z + q1Last * gyro.y - q2Last * gyro.x) * halfT;
	normalise = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
	q0 *= normalise;
	q1 *= normalise;
	q2 *= normalise;
	q3 *= normalise;

The second step is finding error between quaternion calculated by accelerometer directly and quaternion calculated through the Runge-Kutta method. By multiplying quaternions calculated through the Runge-kutta method, theoretical gravitational acceleration is transformed from Earth-fixed coordination system to Aircraft body coordination system. It is important to note that the result matrix is affected by gyroscope’s information. Then we cross multiply the result matrix and accelerometer’s reading matrix. Since both matrices are normalized, what we find is the sine value of angle between rotation axis of the Runge-Kutta’s quaternions and accelerometer’s quaternions. In other words, we are trying to find difference between information detected by gyroscope and accelerometer.

/*through vector cross product to show measured attitude error of 
               gyrpscope */
		ex = (acc.y * rMat[2][2] - acc.z * rMat[2][1]);
		ey = (acc.z * rMat[2][0] - acc.x * rMat[2][2]);
		ez = (acc.x * rMat[2][1] - acc.y * rMat[2][0]);

The third step is fusing attitude derived from gyroscope and accelerometer via PI controller.

/*intergration of error*/
		exInt += Ki * ex * dt ;  
		eyInt += Ki * ey * dt ;
		ezInt += Ki * ez * dt ;
		/*PI controller to fuse data*/
		gyro.x += Kp * ex + exInt;
		gyro.y += Kp * ey + eyInt;
		gyro.z += Kp * ez + ezInt;

The Mahony filter's code:

 void imuUpdate(Axis3f acc, Axis3f gyro, state_t *state , float dt)
{
	float normalise;
	float ex, ey, ez;
	float halfT = 0.5f * dt;
	float accBuf[3] = {0.f};
	Axis3f tempacc = acc;
	gyro.x = gyro.x * DEG2RAD;
	gyro.y = gyro.y * DEG2RAD;
	gyro.z = gyro.z * DEG2RAD;
  
	/* if the three-axis accelerometer is operating, then use Mahony filter*/
	if((acc.x != 0.0f) || (acc.y != 0.0f) || (acc.z != 0.0f))
	{
		normalise = invSqrt(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
		acc.x *= normalise;
		acc.y *= normalise;
		acc.z *= normalise;
  
		/*through vector cross product to show measured attitude error of 
               gyrpscope */
		ex = (acc.y * rMat[2][2] - acc.z * rMat[2][1]);
		ey = (acc.z * rMat[2][0] - acc.x * rMat[2][2]);
		ez = (acc.x * rMat[2][1] - acc.y * rMat[2][0]);
  
		/*intergration of error*/
		exInt += Ki * ex * dt ;  
		eyInt += Ki * ey * dt ;
		ezInt += Ki * ez * dt ;
  
		/*PI controller to fuse data*/
		gyro.x += Kp * ex + exInt;
		gyro.y += Kp * ey + eyInt;
		gyro.z += Kp * ez + ezInt;
	}
  
	/*update quaternion through Runge–Kutta methods in discrete system*/
	float q0Last = q0;
	float q1Last = q1;
	float q2Last = q2;
	float q3Last = q3;
  
	q0 += (-q1Last * gyro.x - q2Last * gyro.y - q3Last * gyro.z) * halfT;
	q1 += ( q0Last * gyro.x + q2Last * gyro.z - q3Last * gyro.y) * halfT;
	q2 += ( q0Last * gyro.y - q1Last * gyro.z + q3Last * gyro.x) * halfT;
	q3 += ( q0Last * gyro.z + q1Last * gyro.y - q2Last * gyro.x) * halfT;
  
	normalise = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  
	q0 *= normalise;
	q1 *= normalise;
	q2 *= normalise;
	q3 *= normalise;
  
	/*calculate rotation matrix*/
	imuComputeRotationMatrix();	
  
	/*calculate euler angle from quaternion*/
	state->attitude.pitch = -asinf(rMat[2][0]) * RAD2DEG; 
	state->attitude.roll = atan2f(rMat[2][1], rMat[2][2]) * RAD2DEG;
	state->attitude.yaw = atan2f(rMat[1][0], rMat[0][0]) * RAD2DEG;
}

Experiment

Since I don’t have any accurate instrument to measure the true attitude of quadcopter, I would like to do a simple experiment instead. The simple experimental procedures: the Minifly open-source quadcopter is placed on a horizontal plane at beginning, then lean forward and backward approximately 45 degree, respectively. Host computer can then record the pitch’s change during the whole experiment, which is estimated through the Kalman filter and the Mahony filter, respectively. Although this experiment seems to be simple, we can still get interesting inspiration about these two filters’ feature.

From the Kalman filter’s diagram, we can find the diagrams of leaning forward and backward 45 degrees are really harsh, and the deviation can even be 8 degrees. The reason is that the quadcopter is leaned by my hand during the whole experiment, and my hand would be shaking. We can find the last part of the second diagram is much smoother. Because at that period, the quadcopter is placed on the original horizontal plane, there is no human intervention.

The diagrams shown above are the experiment using the Mahony filter, we can find that although this experiment is also manipulated by my hand, the diagram is much smoother than the Kalman filter’s diagrams. It is the effect of using PI controller in the Mahony filter.

Conclusion

The Kalman filter is famous for its fast convergence and accuracy, so it is a really effective algorithm to estimate the true physical status. From above experiment, we can find that the Kalman filter detected every vibration. However, in attitude algorithm, real physical status doesn’t mean real attitude. Because when quadcopter is operating, it is heavily interfered by motor vibration and airflow. If attitude algorithm counted these interferences in attitude, the quadcopter will be hard to stabilize. The Mahony filter can promise accurate attitude while filter these interferences, so it is more popular in attitude algorithm.

Reference:

https://www.kalmanfilter.net/kalman1d.html

https://en.wikipedia.org/wiki/Kalman_filter

Introduction to Multicopter Design and Control, Quan quan