The PX4 autopilot is an amazing open source platform for research. It is one of the first open source autopilots capable of running an on-board extended kalman filter and other advanced control and navigation algorithms. It is also mass produced by 3D Robotics and very affordable.

# Hardware Accelerated Extended Kalman Filter

Recently I have completed a C++ matrix library wrapper around the CMSIS digital signal processing library. This means we can now type matrix math like this:

// continuous covariance prediction

P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt;// attitude correction

Vector y = zAtt - zAttHat; // residual

Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance

Matrix K = P * HAtt.transpose() * S.inverse();

Vector xCorrect = K * y;

P = P - K * HAtt * P;// attitude fault detection

float beta = y.dot(S.inverse() * y);// position correction

Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance

Matrix K = P * HPos.transpose() * S.inverse();

Vector xCorrect = K * y;

P = P - K * HPos * P;// position fault detection

float beta = y.dot(S.inverse() * y);

Not only is this very easy to read (similar to Matlab/ScicosLab), it is also hardware accelerated! Thanks to the CMSIS library, multiple floating point operations are executed in one CPU cycle. The result is running a 9 state, 9 measurement discrete time extended kalman filter only consumes 20% of the ARM cortex M4 processor.

You can see the entire example here.

# Sensor Level HIL

In order to develop and test the EKF I also developed the capability for full sensor-level hardware-in-the-loop testing. Simulated sensor data is sent from the flight simulator directly to the autopilot. This means you can run a very high fidelity UAV simulation while sitting at your desk.

You can see the python script to run PX4 hardware in the loop here.

# Object Oriented Control Library

For those that have followed my development, you know that I am a big fan of object-oriented code. While developing the fixed-wing autopilot, I also created a new control library that has a similar feel to what I wrote for ArduPilotOne. This makes it easy for control engineers and the like , who are familiar with block diagram control systems, to easily translate their ideas to code.

You can see an example here for fixed wing.

The features above are just some that I have recently contributed. There are many developers working on PX4 from around the world and many new developments happening every day! Soon a very powerful optical flow board will be released! I hope that you will join this great community!

## Comments

The link for the hardware accelerated kalman filter isn't working. Can you provide it again?

most stable : apm 2.6

most powerful: pix hawk/ px4 (these are basically the same, think of the pix hawk as px4 2.0)

vrbrain: I don't have much experience with.

Last 3 pilot, I've not understood if are same hw/sw !

Give me light on doubts!

Thanks

Hey folks,

I wanted to ask what is the process model that you recommend for EKF attitude estimation, and how do you find the optimal covariance matrices the Q and R. Are they time varying or constant ones?

Thanks.

That's really great contributions. I am personally amazed by this great effort.

I believe this is the right place to seek help from. I am also working on attitude estimation, but not as a major task in my research. However, I definitely need to have as optimal attitude estimation as possible in order to have a better control on my UAV. In my research, I am using c-17 GlobeMaster UAV. I know it's dynamically unstable by nature, but guess what ?! We managed to fly it a full circuit using the autopilot bu Ardupilot mega 2.

However, we require more accurate attitude estimation rather than the origina AHRS algorithm that is already implemented. So I personally don't like to go deep in the programming details regarding the hardware as my job is more focused on the higher level control, but still no way to escape. We definitely need better estimation.

So to be clear, I am dividing my work into two tasks.

Firstly, is to implement EKF/UKF in MATLAB, interface APM2 with MATLAB, and check the performance.

Secondly, implement the UKF/EKF on an external embedded computer (gumstix) that will receive the raw measurement from APM, run the filter, and send back the estimated attitude.

I finished writing the EKF and UKF based on the FOAM algorithm, and they are working fine on offline raw measurements.

The terrible and frustrating problem is that I cannot get it running right, and producing the desired performance on real-time.

I already interfaced the APM to MATLAB using USB (serial communication) and then I process the data on my laptop, and , finally, i visualize the estimated attitude using Virtual reality tool box.

Unfortunately I cannot get the desired performance, and the UKF/EKF is completely usless.

I need your help in order to figure out the problem.

1- Is it a communication problem?! (packet drops)

2- is it wrong usage of the coordinate systems of the sensors

3- Do I need to keep tune the co-variance matrices?

lots of question marks here!!

I already wrote a discussion in order to get this problem solved, and I am ready to share my framework in order to benefit all the public community. Thanks a lot,

Link to my discussion:

http://diydrones.com/forum/topics/building-a-kalman-filtering-frame...

I am happy that my small question started so good thread :-)

If only I was following better the mathematics course instead of thinking about girls :-)

Angelo

@Bill

Ok thanks for that, the example helped.

By psi_error I mean delta, the angle between the measured and 'expected' horizontal vector. As per my previous comment, the magnitude of the cross product = |Hxy| |Mxy| sin(psi_error) ] but since H and M are normalised, the cross product magnitude = sin(psi_error), as you state. So we're calculating the same thing but in different ways.

Cheers,

Dan

@Dan,

I am not sure what you mean by psi_error.

If you mean the difference between the actual and estimated value of the psi Euler angle, that is not what I am computing, that is something else entirely. What I am computing is the sine of the angle between the measured and reference horizontal magnetic field in the earth frame. Unless the aircraft happens to be perfectly level, that is not equal to psi_error. However, if the aircraft is almost level, then the angle error that I am computing is approximately equal to psi_error. If the aircraft is vertical, neither psi or psi_error are defined.

By the way, there is a detail I should mention: Before I take the cross product, I normalize both vectors by dividing them by their magnitudes. So the result of the computation is sine(delta), where delta is the angle you would need to rotate the estimate of the aircraft orientation around the earth frame vertical axis to correct it. For small errors, sine(delta) is approximately equal to delta.

At this point in the calculation, you can either view delta as a scaler, or as the z component of a vector whose x and y components are zero. The vector represents an orientation rotation error in the earth frame.

Projecting the earth frame rotation orientation error vector back to the body frame is easy. You simply multiply delta by the earth vertical axis as seen in body frame. In other words, multiply delta by the third row of the direction cosine matrix. This gives you a three component vector. Each component of this body frame rotation orientation error vector is the rotation orientation error of one of the axes of the body frame.

Perhaps an example will help.

Consider an aircraft oriented vertically with the nose pointed straight up, and suppose that its roll orientation in the body frame is off by 0.1 radian (about 6 degrees).

Then, the reference and measured horizontal magnetic field in the earth frame will be misaligned by 0.1 radian, and the cross product of the normalized vectors will be 0.1, representing a rotation orientation error in the earth frame of 0.1 radian around the vertical axis.

Next, transform this [0, 0, 0.1] vector into the body frame where it will become [0.1 , 0 , 0]. In other words, it will indicate a roll rotation orientation error in the body frame.

Finally, use whatever estimation process you like to take the roll rotation error in body frame and adjust the body roll orientation.

Notice that what has happened is that the yaw orientation rotation error in the earth frame becomes a roll orientation rotation error in the body frame.

Best regards,

Bill

@Bill

Thanks for the explanation. I believe your method is a elegant way to calculate the psi error (if desired) without calculating the magnetometer heading directly, i.e.

Hxy (cross) Mxy = [ 0; 0; |Hxy| |Mxy| sin(psi_error) ]

Where Hxy is the horizontal component of the magnetometer measurement in the Earth frame and Mxy is the horizontal component of the reference magnetic vector in the Earth frame (declination vector). If so, how do you correct all three axes using this? When you rotate this vector back to the body frame this vector is still perpendicular to the horizontal frame and is really just a scalar value? I would guess that all 3 axes could be corrected using the cross product of the full 3D measurement and 3D reference vector i.e.

H (cross) M

Please bear with me as I don't know how you are using the result to correct the drift.

Cheers,

Dan

@Dan,

Regarding using the magnetometer for yaw drift compensation, I am not sure if we are talking about the same thing or not. I am not using the tilt compensation method described here, which is the common way of doing it. The problem with that method is that it does not work at all for a pitch angle of 90 degrees, it falls apart completely, and provides no yaw drift information. For vertical pitch (theta=90), the roll angle becomes indeterminant, causes the yaw angle estimate to also become indeterminant. So tilt compensation cannot be used for VTOL.

The method that I use computes an orientation error using only the horizontal component of magnetic field, but it does not interpret it as a "yaw" error, rather it is interpreted as an orientation error, and it is projected on, and applied to, all three axes in the body frame, not just the "yaw" axis in the body frame.

The problem becomes clearer when you consider the case of a vertical attitude. In that case, the roll axis of the aircraft aligns with the yaw axis of the earth. So for that orientation, the magnetometer is providing a roll reference in the body frame, not a yaw reference. The trap is in thinking in terms of Euler angles.

The method that I am using is quite different from tilt compensation, because it does not use Euler angles at all.

The core issue is the use of Euler angles, they really are not suitable for aviation applications because of gimbal lock. Instead, it is better to use either quaternions or rotation matrices.

Best regards,

Bill

Next