PX4 Autopilot: New Software! Hardware Accelerated Extended Kalman Filter/ Sensor Level HIL/ OO Control Library

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!

Views: 11486

Comment by Dan Wilson on January 23, 2013 at 4:56pm

@James Cheers, I've also been following your work on JSBSim - awesome stuff! I currently have a H for both GPS/mag/baro and mag/baro and use the GPS version when new GPS data is available. Alternatively you can do a sequential update with a separate H for GPS and mag/baro i.e. when all sensor data is available in a single timestep update with GPS first and then mag/baro directly afterwards. I have implemented both and accuracy differences are negligible. I'll be interested to see your results when updating with accel vs not. I have tried both and gained better results without the accel, however you will probably get a better attitude estimate in the static case when you use the accel update. Something else worth mentioning - updating with the heading as calculated by the mag (+ declination) is better than updating with the raw magnetometer 3D vector because the full 3D vector corrects all the attitude states. This means that any magnetic disturbances will perturb phi, theta and psi rather than just psi. Other things to think about for the future:

  • Estimating attitude using quaternions rather than euler angles to avoid gimbal lock (if you're not already)
  • Better prediction integration (Runge-Kutta etc) although this is more computationally expensive
  • GPS latency - there is a nice way to deal with this when using a UKF formulation
  • Implementing a UKF - I haven't done this yet but,
    • Pros:
      • Can approximate 2nd order nonlinearities (rather than first order for EKF)
      • No need to derive Jacobians
      • More robust to initialisation errors
      • Elegant way to handle sensor latency
    • Cons:
      • More computationally expensive
      • Bit more complicated

@Bill I agree that small inaccuracies in IMU scale factors can have large consequences, particularly for highly dynamic vehicles. Thankfully I have never had to deal with this since my vehicles are rather benign. In addition to a good offline calibration you could add the scale factors to the EKF state vector and estimate them online, much in the same way that you do with the IMU bias. The downside, of course, is extra computation.


T3
Comment by William Premerlani on January 23, 2013 at 6:06pm

@Dan,

I agree with your comments about gimbal lock, and also your comments about updating with magnetometer data. That implies that mag data should not be used to update the psi Euler angle, particularly if you plan to fly an aircraft both horizontally and vertically. If you use mag data to estimate the error in psi, the magnetometer becomes useless when the aircraft is vertical. Here is what we do in MatrixPilot instead:

1. Transform magnetometer vector from body frame to earth frame.

2. Take the cross product of horizontal component of magnetometer vector in the earth frame, with the declination vector, producing a yaw error vector in the earth frame.

3. Transform the earth frame yaw error vector back to the body frame, and use it to provide drift compensation for all three rotation axes.

In other words, we do not treat the magnetometer as a tilt-compensated compass.

We do not use Euler angles in MatrixPilot. These days we have a mix of quaternion based computations, and rotation matrix based computations.

Best regards,

Bill

Comment by Dan Wilson on January 23, 2013 at 7:38pm

@Bill,

It sounds like 1. and 2. are an indirect method of my approach. By finding the cross product of the horizontal component of the derotated mag vector and the declination vector (which I assume does not include the inclination and is thus also the horizontal component) the result is always a vector in the earth frame vertical axis, whose magnitude is the cosine of the heading error (multiplied by the magnitude of the mag and declination vectors). This sounds similar to directly calculating the heading error. 

I don't know how you do the drift compensation so I'll have to take the time to have a detailed look at MatrixPilot particularly the navigation algorithms!


T3
Comment by William Premerlani on January 24, 2013 at 7:26am

@Dan,

I think there are some similarities between what you and James are doing and some of the extensions I have made to DCM. There is possibly an equivalence relation for some of it. For example, it looks like the way that I account for acceleration is the derivative of the way James does it. But then the error vector that I compute gets fed into an integrator, so it may be that the overall result is approximately the same.

A major difference between EKF and complementary filters is the inverse of the covariance matrix used by EKF.

Best regards,

Bill


T3
Comment by William Premerlani on January 24, 2013 at 7:58am

@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

Comment by Dan Wilson on January 24, 2013 at 5:26pm

@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

 


T3
Comment by William Premerlani on January 24, 2013 at 8:10pm

@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

Comment by Dan Wilson on January 24, 2013 at 9:17pm

@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

Comment by Angelo DP on January 25, 2013 at 12:52am

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

Comment by Mohamed Abdelkader Zahana on April 18, 2013 at 8:49am

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...

Comment

You need to be a member of DIY Drones to add comments!

Join DIY Drones

© 2019   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service