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
@James,
Regarding acceleration, I agree that my equation 3 is consistent with EKF velocity propagation. In other words, we are agreed on what an accelerometer actually measures. Where we diverge is how we use velocity for tilt drift compensation. You are using velocity directly, I am taking its time derivative to work in terms of acceleration. In my opinion, that is the better way to go, because we are using gravity as the vertical reference vector, and gravity is an acceleration, not a velocity. But then, I might be missing something in your EKF, and maybe I am being stubborn as well. A fair test of your method would be to see if it automatically compensates for centrifugal effects during a sustained turn, with a small but time varying gyro offset. I will keep an open mind until you do some simulation testing.
Tridge has done simulations of my method, and reports that it works, as long as there is not much error in the yaw estimate.
Best regards,
Bill
Hey guys,
You can avoid calculating/compensating for centripetal acceleration etc but not updating with the accelerometers. The attitude is corrected naturally within the GPS/barometer/magnetometer update when the attitude is used to form the rotation matrix to transform between the IMU body frame and the global GPS frame. James, this is basically what you were saying in your comment about correcting with GPS velocity. For this to work with hovering multicopters you would need a good magnetometer heading measurement.
I would also suggest including the gyro and accelerometer bias' (at least the gyro bias) in the state vector, modelled as a random walk. It's fairly simple to implement and works really well, converging in a few seconds. Doing so eliminates the need for IMU zeroing during initialisation.
Hope this helps.
Here is why I think the EKF is doing something similar to your algorithm but in a more robust way. Granted your way is a good way to do it without a combined position/velocity/attitude EKF.
If we start with the EKF navigation velocity propagation:
V1 = V0 + int(C_nb * a_b, t, t1, t2)
The can be rearranged to Eq 3:
int(C_nb * A_b, t, t1, t2) = -(t2-t1)*g - (V2 - V1)
But the V here is the predicted velocity not the GPS velocity.
Now for the correction, I think the kalman filter is doing something different. If we use Vg for GPS velocity:
(new V2 estimate) = (kalman gain mapping error to state correction) * (Vg2 - V2)
(corrected attitude)
And we know for the linear case that it is "optimal". So it might not be doing the exact same as what you are suggesting but I think that it should be more robust. If the GPS velocity suddenly gives an erroneous measurement, the EKF will do a better job of rejecting it.
And as you suggest, I should not use accel vector correction at all for the quad if this velocity correction is helping it anyways. We will have to test it out and see at some point I guess :-).
Maybe I'm still completely missing something on your error correction that the EKF doesn't account for, if so, forgive me for being stubborn. I really want to make sure we are doing this right.
@James,
I took a closer look at equation 4 of my paper on the subject of acceleration compensation. There are some differences in our two methods, I do not think they are the same. Your method is basically based on velocity, my method is basically based on acceleration. Two different concepts.
The core of my method is equation 2, which says that applying the estimated rotation matrix to the body frame accelerometer measurement vector should produce a vector that aligns with gravity minus acceleration in the earth frame. This is an identity, no matter what the acceleration is. Any misalignment between the two vectors is due to attitude error in the estimated rotation matrix.
To improve signal to noise, I integrate equation 2 between GPS readings. This is a very short time interval, so its not the same as integrating to get velocity. Its really just a filtered acceleration measure.
Another way to look at it is to consider my equation 3, for the case where t1 = 0, V(t1) = 0.
Equation 3 is an identity. If you rearrange it, you will see that the true velocity vector in the earth (GPS) frame is equal to the integral of the true rotation matrix times the true accelerometer vector in body frame, minus gravity vector times time.
The point is, the integral smears out the effects of time varying attitude errors. In effect, only the average of attitude error for the entire flight can be estimated, not the most recent value.
So, I do not think that comparing GPS velocity with IMU velocity is a substitute for acceleration compensation. I think the choices still are:
1. for fixed wing, use omega cross velocity.
2. for quads, use my method, or use nothing at all.
Best regards,
Bill
I think it is equivalent to the time integral, but will have to look at it more closely. Good point on the latency :-). I should add that to my sensor hardware-in-the-loop model as well.
@James,
I am not sure if we our approaches are equivalent or not. I am not using velocity to correct attitude. Rather, I am using change in velocity as a reference vector to compare with the integral of acceleration, which a different thing from what you are doing. Perhaps my method is equivalent to the time integral of your method. The only way to know for sure if your method works is to test it, and see if it automatically compensates for acceleration.
Also, if your method is equivalent to my method, be aware that it requires accurate data from magnetometer. So, for example, there is a problem if declination is not right. That is because there are several combinations of yaw, roll, pitch error that cause the GPS velocity to exactly align with IMU velocity.
By the way, you are going to have to somehow account for GPS latency during a turn, because the reported direction of GPS velocity will not align with true navigation velocity. Typical GPS latency is 1 second. During a turn at even a snail's pace of 10 degrees per second, (36 seconds for a full circle) there will be a misalignment of 10 degrees!
Best regards,
Bill
@Bill: I think I have wrapped my head around your equations. From what I can tell you are using the GPS velocity measurements to correct the attitude. This correction happens naturally in the combined position/attitude EKF. When GPS velocity information is received, if it is not correctly aligned with the current predicted navigation velocity, it corrects the predicted attitude. I do have this correction currently commented out, but can now see the benefit of including it. Please correct me if I misunderstood.
James,
Regarding your EKF algorithm, I really like the fact that you are using EKF to simultaneously estimate attitude, velocity, and position. That is the best way to go, I think. Good job. I really am excited to see that you are making EKF accessible to our community. Way to go! Thank you.
I think that EKF has the potential for delivering very accurate results, but you may have to explicitly address some sources of error, which EKF might or might not be able to compensate for. Some things to consider:
1. GPS latency. A high GPS reporting rate is no guarantee of low latency. It is my understanding that even with reporting rates of 10 Hz, latency can be as high as 1 second.
2. Magnetometer reporting latency.
3. Gyro gain errors.
4. "Dizzy" effects during sustained high rotation rates.
5. Magnetometer misalignment.
6. Error in magnetometer declination parameter.
7. Misalignment between IMU and aircraft axes.
8. Sensor offsets.
9. Pitot gain error.
10. Acceleration compensation. (This is going to be tough for quads.)
No doubt EKF will take care of some of these errors, but some of them might dilute the accuracy of any algorithm, unless they are explicitly addressed.
It might be interesting for you to do some simulations in which you introduce these sorts of errors, and which you will be able to know what the "truth" is, and see how well EKF is, or is not, able to compensate for these sorts of sources of error. I would especially like to know what happens if you put the aircraft in a simulated high acceleration turn at say 360 degrees per second, for a long period of time, say 10 minutes, and then straighten out into level flight, and see whether or not the algorithm gets "dizzy".
Best regards,
Bill
OK, yeah makes sense how a quad moving forward spinning on it's axis would totally mess up the centrip accel prediction. Thanks again for the resources. I don't have time to tackle this right now, but will get into it soon.
Hi James,
Regarding acceleration compensation...
Fixed wing is easy, quads is not. Lets talk about them separately.
For fixed wing, the best way really, is to compute centripetal acceleration as the cross product of rotation rate vector with air velocity vector.This is the method we use in MatrixPilot, and I am pretty sure it is also the method used in ArduPlane. Rotation rate you get from the gyros. You can get magnitude of air velocity from pitot tube. For the direction of air velocity vector, to a first approximation, you can assume its parallel with the fuselage. For a second approximation, you can compute angle of attack and side slip angle from an aerodynamic model of the aircraft.
For forward acceleration, take the time rate of change of the airspeed.
The reason the above method works for fixed wing, is that a fixed wing moves with respect to the air approximately in the direction that it is pointed. Not so for a quad.
For quads, the best guy to talk to is Tridge, but I think he will tell you that for quads, you have two choices:
1. Ignore acceleration, and do not do any compensation.
2. Use the compensation method that I came up with, described here:
In any case, I recommend you talk to Tridge. He was the first one to implement my method for acceleration compensation for quads. At first it seemed to work really well. Then Tridge discovered some problems, which I think he fixed by re-scaling the vectors used in the computation a little differently from the way described in my published report.
Best regards,
Bill
-
1
-
2
-
3
-
4
-
5
of 6 Next