Despite of the lack of fresh messages on my weblog (no, I’m not abandoning the hobby UAV scene :-) ), I still get a lot of emails with questions and requests.
Some people emailed me for the code of the artificial horizon I published some time ago. Unfortunately that one was a quick hack and I was planning to rewrite it in clean and well documented .Net code and post them on my weblog. But honestly, so far it works for me and probably will for you, so why should I put more time into it? :-)
So: here’s the VB.net code: ArtificialHorizon.zip
Usage: Build it, drag the user component on your form, update pitch and roll properties.
This weekend, I made a bad PCB. The only option that was left, was creating a new one. But my PIC was already soldered on! After a search on the net, I didn’t find any approach with simple tools to desolder an SMD chip (in a way that it will still work afterwards!).
This and this are brilliant examples of how not to do it :-)
So I came up with my own solution:
- Put a lot of solder on the 4 sides of the chip.
- Turn an iron (to iron your clothes) upside down on maximum temperature.
- Put your PCB on it (no solder on the bottom).
- Now quickly heat up the 4 soldered sides with your soldering iron.
- The heat of the iron will make sure the solder remains liquid for a few seconds.
- You can use your soldering iron to push the chip off the PCB when you’re desoldering the last side.
- Do it quick so your chip won’t be burnt!
Good luck :-)
To many of us, kalman filtering is something like the holy grail. Indeed, it miraculously solves some problems which are otherwise hard to get a hold on. But beware, kalman filtering is not a silver bullet and won’t solve all of your problems!
This article will explain how Kalman filtering works. We’ll use a more practical approach to avoid the boring theory, which is hard to understand anyway. Since most of you will only use it for MAV/UAV applications, I’ll try to make it look more concrete instead of puzzling generalized approach.
Make sure you know from the previous articles how the data from “accelerometers” and “gyroscopes” are used. Some basic knowledge of algebra may also come in handy :-)
Kalman filtering is an iterative filter that requires two things.
First of all, you will need some kind of input (from one or more sources) that you can turn into a prediction of the desired output using only linear calculations. In other words, we will need a lineair model of our problem.
Secondly, you will need another input. This can be the real world value of the predicted one, or something that is a good approximation of it.
Every iteration, the kalman filter will change the variables in our lineair model a bit, so the output of our linear model will be closer to the second input.
Our simple model
Obviously, our two inputs will consist of the gyroscope and accelerometer data. The model using the gyroscope data looks like this:
The first formula represents the general form of a linear model. We need to “fill in” the A en B matrix, and choose a state x. The variable u represents the input. In our case this will be the gyroscope’s data. Remember how we integrate? We just add the NORMALIZED measurements up:
alpha k = alpha k-1 + (_u_ k – bias)
We need to include the time between two measurements (_dt_) because we are dealing with the rate (degrees/s):
alpha k = alpha k-1 + (_u_ k – bias) * dt
We rewrite it:
alpha k = alpha k-1 – bias * dt + u k * dt
Which is what we have in our matrix multiplication. Remark that our bias remains constant! In the tutorial on gyroscopes, we saw that the bias drifts. Well, here comes the kalman-magic: the filter will adjust the bias in each iteration by comparing the result with the accelerometer’s output (our second input)! Great!
Wrapping it all up
Now all we need are the bits and bolts that actually do the magic! These are some formulas using matrix algebra and statistics. No need right now to know the details of it. Here they are:
|u = measurement1||Read the value of the last measurement|
|x = A · x + B · u||Update the state x of our model|
|y = measurement2||Read the value of the second measurement/real value. Here this will be the angle calculated from our accelerometer.|
|Inn = y – C · x||Calculate the difference between the second value and the value predicted by our model. This is called the innovation|
|s = C · P · C’ + Sz||Calculate the covariance|
|K = A · P · C’ · inv(_s_)||Calculate the kalman gain|
|x = x + K · Inn||Correct the prediction of the state|
|P = A · P · A’ – K · C · P · A’ + Sw||Calculate the covariance of the prediction error|
The C matrix is the one that extracts the ouput from the state matrix. In our case, this is (1 0)’ :
alpha = C · x
Sz is the measurement process noise covariance: Sz = E(zk zkT)
In our example, this is how much jitter we expect on our accelerometer’s data.
Sw is the process noise covariance matrix (a 2×2 matrix here): Sw = E(x · xT)
Thus: Sw = E( [alpha bias]’ · [alpha bias] )
Since only the diagonal elements of the Sw matrix are being used, we’ll only need to know E(alpha2) and E(bias2), which is the 2nd moment. To calculate those values, we’ll need to look at our model: The noise in alpha comes from the gyroscope and is multiplied by dt2. Thus: E(alpha2) = E(u2)· dt2.
These factors depend on the sensors you’re using. You’ll need to figure them out by doing some experiments. In the source code of the autopilot/rotomotion kalman filtering, they use the following constants:
E(alpha2) = 0.001
E(bias2) = 0.003
Sz = 0.3 (radians = 17.2 degrees)
This tutorial descibes how an accelerometer can be used to determine an aircraft’s attitude (pitch and roll).
An accelerometer measures, as it’s name hints, acceleration along a predefined axis. As you probably remember from you physics class, the earth’s gravity is also an acceleration (a falling stone keeps going faster and faster). So: with an accelerometer, we can measure the earth’s gravity! This image shows how we do it:
The red arrow represents the earth’s gravity. The blue arrow shows how the accelerometer senses gravity. Note that the axis of this accelerometer is perpendicular to the aircraft (we placed it like that in our aircraft!).
The angle theta between the actual gravity vector and the measured gravity is related to the pitch of the aircraft (pitch = theta + 90°). If we know theta, we know our pitch! Since we know the magnetude of the earth’s gravity, simple calculus gives us our pitch angle:
accelerometer = cos (theta) * gravity
theta = acos (accelerometer / gravity)
And since pitch = theta + 90°
pitch = asin (accelerometer / gravity)
Woooow, we calculated the pitch orientation of our airplane using an accelerometer. Pretty easy, huh?
Calculating the roll angle is pretty much the same. We only need an extra accelerometer with an axis perpendicular to the pitch-accelerometer.
Reality is a bit different from this simplified example. The inverse sinus can’t give you the full 360 degrees ranging pitch angle. A plane heading for the sky and one heading for the ground would both result in a 0 (zero) measurement. We’ll need an extra accelerometer to distinguish these cases. The 2-argument inverse tangens makes sure the resulting angle is in the correct quandrant. Thus:
pitch = atan2(accelerometer / gravity, z / gravity)
Now you know most about using accelerometers to calculate pitch and roll, don’t start building your own autopilot system just yet! There are more forces working on a flying airplane then just good old gravity! Just think about the centripetal force when following a circle path. We’ll need gyroscopes to correct this over short period of time (also usefull to eliminate the effect of vibrations on the accelerometer). Over a longer period of time, we’ll need some more advanced physics to estimate these other forces so we can compensate for them. I’ll write later about the different approaches you can use to do this.
The gyroscopes are covered in the next tutorial: gyroscope to roll, pitch and yaw
Every good autopilot system needs an IMU-module. IMU stands for Inertial Measurement Unit. Basically, you can look at it as a black box that gives information concerning your current position and orientation. Position could be GPS-coordinates. Orientation could be roll, pitch and yaw. Sometimes you need to calculate it yourself, sometimes the black box calculates it for you. Soooo, since an autopilot needs to know the current position and orientation, it needs an IMU.
|The 6DOF by SparkFun electronics. Notice the 2 gyroscopes standing in upright position. One for each axis!|
Every interested mind now wonders “how does this black box work?”
Well, I’ll give you the three most importent sensors.
The first one is, of course the GPS. It gives you your current position and your speed.
The second one is a set of 3 accelerometers. Each one senses acceleration in one direction. Remember from your physics class that gravity is also an acceleration! So the 3 accelorometers give you the 3 components of the gravity vector! Knowing that the gravity vector is supposed to point right to the middle of the earth, you can calculate the orientation! Keep in mind that when an MAV is flying, other forces like centripetal acceleration may badly influence those 3 accelerometers used to sense the gravity vector. As a result those measurements are only correct when averaged over a longer period of time.
The last one is a set of 2 or 3 gyroscopes. Thats a very fancy word for a sensor that senses the speed of rotation (also called angular velocity). With 3 gyroscopes you can sense the speed of rotation around your 3 (x, y, z) axes. Clever readers may notice that the mathematical integration of the values give you the orientation (integration of speed = position)! Unfortunately, there is a lot of drift on those values, so they are only correct for a short period of time…
Readers who payed attention noticed that the orientation given by the accelerometers are correct over a long period of time, and the one given by the gyroscopes are correct over a short period of time. Can’t be combine those two?! Yes we can! A very special filter called the kalman filter does the trick.
Nowadays, every guided vehicle (from a guided missile to a jumbojet) uses those sensors with the kalman filter. Mr. Kalman who invented his filter in the fifties must be so proud!
This page gives an overview of some commercial available IMU’s.
I tried to list the most interesting autopilot systems.
|Paparazzi||/||A DIY project|
|MP2028^g^||MicroPilot||28gr incl. GPS!|
|Phoenix||o-navi||Not including software?|
|TGE||C. C. & design|
|Piccolo||Cloud Cap Tech|
This list is far from complete. Please contact me if you know other autopilot systems.
MicroPilot is a company solely focused on miniature autopilot systems. They also offer various add-on modules.
- Homepage: http://www.micropilot.com
The o-navi company is specialized in navigation modules using MEMS sensors. Their top product is an integration of their IMU module in a full flight controller engine: the Phoenix
- Website: http://www.o-navi.com
Tiny Guidance Engine (TGE)
Continental Controls & Design is a small company with one product: a small autopilot system. This includes the IMU-module with microcontrollor and ground control software. Their main client is the US army.
- Website: http://www.continentalctrls.com
Cloud Cap Technology is a company with UAV-autopilot related modules as only interest. Their autopilot look very professional but big and heavy, compared to their IMU-modules.
- Homepage: http://www.cloudcaptech.com
This project aims to develop an autopilot under the GNU public license. While not that advanced (eg. stabilization is done using 4 thermophile sensors), it has proven to be very succesfull.
- Homepage: http://www.nongnu.org/paparazzi/
The roots of the rotomotion company lie in the autopilot project on sourceforge. Now, they sell a more advanced version of this project. As a plus, they also offer fully equiped UAV helicopters.
- Homepage: http://www.rotomotion.com
This company offers a very lightweight autopilot solution as a composition of various modules. It looks oriënted at the advanced RC-hobbyist.
- Homepage: http://www.u-nav.com
I tried to list the most interesting commercial available IMU-module. These can be interesting if you plan to develop your own autopilot(-software).
|uNAV||Crossbow||57×45×26||33g||Open source software|
|TGE||C. C. & design||30×30×18||13g||Full autopilot?|
|Crista Sensor||Cloud Cap Tech||28×29×15||8,5g|
|6 DOF||Sparkfun El.||51×51×23||21g||With bluetooth link|
|MTi||Xsens||58×58×22||50g||RS232 & USB|
I know this list is far from complete. Please contact me if you know other commercial available IMU’s.