Being involved in UAV research, I've been a member and a great admirer of this community for a number of years. An article I wrote (Fundamentals of Small Unmanned Aircraft Flight) has recently been published in my organization's Technical Digest and is freely available. I am hoping that members of the DIY UAV community might benefit from this effort.
The article covers flight control methods as well as UAV state estimation methods (i.e. fusing sensors to estimate position and attitude). Flight control algorithms are treated very generally, basically just describing simple PID control for different flight modes and channels. For state estimation, the article covers Kalman Filter-based approaches as well as the Mahoney/Premerlani/Bizard "DCM" method that many in this community are familiar with. (In the article, I refer to the "DCM" method as the AHRS Feedback Controller method, as it can also be described using quaternions or euler angles.)
Although the focus of the article is small fixed-wing UAVs, many of the methodologies described can be (and are) applied to other types and sizes of UAVs. The flight control and state estimation methods described are not specific to any particular UAV, but I will admit that I leveraged resources from the open-source UAV community. The article is freely available at the following link:
www.jhuapl.edu/techdigest/TD/td3102/31_02-Barton.pdf
As a companion to the article, I also put together some heavily-commented MATLAB routines (called SUAS Code) implementing both the Kalman Filter and the "DCM" Feedback Controller state estimation methods. (For the really interested user, the code shows how to implement the "DCM" Feedback Controller method using either Euler angles, quaternions, or Direction Cosine Matrices.) A link to the code can be accessed through the MATLAB Central Link Exchange:
www.mathworks.com/matlabcentral/linkexchange/links/3474
(The Link Exchange provides a link to the actual SUAS Code website. Please forgive me that this a "link to a link", but the MATLAB Link Exchange counts "clicks", giving me feedback as to whether anyone is finding it useful. Also, please be forewarned that you will have to OK an End-User-License-Agreement before downloading the code. It is intended that the code be freely available for academic, personal education, and research purposes. The EULA really just restricts commercial efforts, so hopefully no one in this community will find it too distasteful.)
Again, I am hoping that the paper and code are of benefit to this community.
Comments
Alright, now I have clear. Another doubt I have is, according to your filter, in order to use some real data, it has to be done filling the "uavSensors.gyro_wb_rps", "uavSensors.accel_fb_mps2" and "uavSensors.mag3D_unitVector_in_body" arrays directly without adding any error because the sensors have them already, Isn't it? In case of the third sensor (magnetometer), Is it necessary to take into account the declination and/or inclination if the true north is not needed precisely? I mean for a drone flight or PID stability control is not really important where is the true north, it just will be wrong value but unless you pretend to fly using GPS checkpoints or use it for a "world wide" navigation is not needed, Am i wrong?
Thanks again!
Yes, when you extract Euler angles, one of them will generally be between [-90, 90], and the other two will be between [-180, 180]. But, as discussed above, you can alter the method to find equivalent orientation representations. (i.e. Euler angles aren't unique)
For your second question: To determine estimation error, you certainly need to know "truth". In simulation, that is easy. In real-world applications, not so much. To do so, you would likely need to invest a lot of money or effort to use an alternative attitude measurement. For indoor applications, many have used the VICON system to provide truth. For outdoor applications, you can feasibly extract attitude from either on-board or external cameras.
av9:
I'm glad your finding the paper and code useful. I believe what you are seeing is not an issue with the filter mechanization, but rather with the Euler angle extraction method. As we extract the roll/pitch/yaw Euler angles from either a rotation matrix or a quaternion, pitch generally ends up being between -90 and +90 degrees, whereas both yaw and roll end up between -180 and +180 degrees. (Or equivalently between 0 and 360 degrees).
However, Euler angle rotations are non-unique, which is good news for you. Thus, you can convert any set of Euler rotations to another equivalent set. Specifically, you can use a pitch-supplemental equivalent:
[ yaw pitch roll ] -> [ yaw+180 180-pitch roll+180 ]
My suggestion is to extract Euler angles in the usual way, and then check if the magnitude of roll exceeds 90 degrees. If so, convert the angles as described above. Finally, I suggest bounding all of the resulting angles between -180 and 180 (although the new roll should actually be bounded between -90 and 90)
Degrees example in Matlab:
bound_180 = @(angle)( mod( angle+180 , 360) - 180 ); % Inline function to make -180<=angle<180
if( abs(roll_deg)>90 )
yaw_deg = bound_180( yaw_deg+180 );
pitch_deg = bound_180( 180-pitch_deg ); % Will be between -180 and 180
roll_deg = bound_180( roll_deg+180 ); % Will be between -90 and 90
end
Hopefully I understood the issue you described, and that this helps.
Really nice work! Congrats! I would like to ask you one question, pitch angle values are within the range [-pi/2, +pi/2] in Euler representation due the singularity when approaching to +-90 degrees. I've tested your code using pre-stored data from my own sensors and no matter which method is used (AHRS-euler, AHRS-quaternion,...) the pitch angle range is from [-pi/2, +pi/2]. I was expecting to avoid this behaviour and the gimbal lock when executing the quaternion algorithm and converting it later to Euler just to show some indicators.
What is wrong on my understanding? Is it impossible to represent pitch angles in Euler representation from [-pi,+pi] even you calculate the orientation using quaternions and converting them to Euler?
Haven;t read it yet. I am sure that will enjoy it. Thanks for sharing!
A timeless piece.
hi jeff,
yes, updating from 3.2 to 3.6.2 solves most issues.
i am surprised how it works!
continue to play with it.
i have a simpler example to filter the accelerometer values. will be very interesting to see the kalman filter in real action.
@robert,
As far as I can tell, qtoctave seems to be discontinued. However, I did try running the Matlab routines using (the free) GNU Octave for Windows. To my pleasant surprise, all the UAV State Estimation Matlab routines ran perfectly on Octave right out of the box. Octave successfully:
- Read the .mat UAV "truth" data
- Generated the synthetic sensor measurements (gyros, accels, etc.)
- Used the user-selected state estimation method to estimate positions and attitudes
- Displayed the data comparing the state estimates with truth
I'm very impressed with GNU Octave that it so readily performed the Matlab scripts with absolutely no modifications. It was quite a bit slower than Matlab, and has a bit clunker of an interface, but it is hard to argue with free!
Just to be specific, I installed Octave 3.6.1 by following the manual installation instructions at http://wiki.octave.org/Octave_for_Windows
@jeff,
i played with qtoctave - so far the mat files cannot be read.
plus the directory handling is different.
need to grab some flight data for transfering it as static data into octave.
have you exported some of your functions into 'c'?
this is the next step.
-
1
-
2
of 2 Next