Two weeks ago, I crashed my micropilot plane. Repearable, but I thought it might be a better idea to use an “easy” plane for the first tests with my autopilot. Also, the limited space available in my delta plane was an issue. And so I bought an easystar. The space under the canopy is very generous, which makes it easy for testing. I installed ailerons on it to make sure it would respond swiftly to my input on its roll-axis.
The first two flights were quiet a disappointment. Flying level posed no problem at all, but making coordinated turns was not as good as expected. Suddenly, I realised I was flying the easystar in 20km/h winds, while the plane only flies at – like 30km/h? Obviously, the pitching behaviour of the eastar when flying upwind or downwind is completely different. I suppose it was just a bad idea to try it in such high winds… Also, the autostabilizing behaviour of the easystar makes it a bad choice as a roll-stabilized plane.
In the late afternoon, when the wind fell between 5 and 10km/h, I did another test. And it was a success! The easystar flew nicely, no weird pitch behaviour anymore, and coordinated turns (and coming out of) were a success! Yiha!
Right now, I just updated my PID code. Now it is much more general. However, this “clean” approach made the update rate drop from 50 updates per second to 41 updates per second. So I was wondering: If you had an autopilot system, would you mind sacrificing performance for clean, readable code? And what about using integer math instead of floating point math (and thus losing the correct “scientific” units)?
No need to worry just yet: my dsPic runs at 7.8MHz, and its capable of 80MHz :-)
I just finished another prototype for my autopilot module. The biggest changes are:
- SMD components!
- Runs on 3.3V: more sensor stability.
- Possibility to use an external crystal instead of the internal oscillator.
The software is finished for about 80-90%. For the moment it looks promising. At only 8MHz (that only 2MIPS!), the module does the following, 50 times per second:
- Decode (and glitch filter) PPM pulse train from RC-receiver.
- Send PWM signals to the servos.
- Sampling of the sensor values and filtering them with a 4th order runge-kutta filter.
- Decode GPS input at 2Hz.
- Kalman filtering pitch and roll.
- A simple PID algorithm for stabilization.
Looks nearly finished… however, testing in a real MAV and finetuning everything will take a lot of time!
A lot of people saw I purchased a 5DOF module from SparkFun and even made a test-PCB for it. Obviously, they asked me if I was willing to share my code. Well alright, here you have it.
A simple embedded kalman filter, nicely documented. Also featuring, for the interested:
- Using the ADC hardware module in a dsPIC30
- Using the UART module in a dsPIC
- Using the timer module in a dsPIC
- How to organize your code! I’ve seen some hideous projects on the web.
The code is written using Microchip’s C30 compiler . The non-optimizing version is free!
I wrote a small app to visualize the output better:
And last but not least: downloads!
- Eagle schematics: 5DOF_Tester_v0.2.zip
- MPLab project files and c-code for the C30 compiler: KalmanTestBoardV1.zip
There is a bug in Microchip’s latest libs. You need to add: #define UART_ALTRX_ALTTX 0xFFE7 /*Communication through ALT pins*/
to uart.h or to the code that references it.
A lot of people think they need great electronics skills, a lot of time, and embedded programming skills to experiment with Kalman filtering of IMU-data. Think again! The key to all your success stories is simulation!
One of the easiest tools that can help you with those simulations, is a great flight sim called Flight gear. It uses advanced aerodynamics simulation libraries, among which some were created by the NASA. Good enough for our purposes! On top of that, you can easily configure Flightgear to log all the data you need. I updated my config file to log the following variables:
- Acceleration along the 3 axis
- Gyro reading along the 3 axis
All the available fields can be found in FlightGear\docs\README.properties
I added the following lines to the configuration xml:
<logging> <log n="0"> <enabled>true</enabled> <interval-ms>100</interval-ms> <filename>fg_log.csv</filename> <delimiter>,</delimiter> <entry n="0"> <enabled>true</enabled> <title>AccX</title> <property>/accelerations/pilot/x-accel-fps_sec</property> </entry> <entry n="1"> <enabled>true</enabled> <title>AccY</title> <property>/accelerations/pilot/y-accel-fps_sec</property> </entry> <entry n="2"> <enabled>true</enabled> <title>AccZ</title> <property>/accelerations/pilot/z-accel-fps_sec</property> </entry> <entry n="3"> <enabled>true</enabled> <title>DRoll</title> <property>/orientation/roll-rate-degps</property> </entry> <entry n="4"> <enabled>true</enabled> <title>DPitch</title> <property>/orientation/pitch-rate-degps</property> </entry> <entry n="5"> <enabled>true</enabled> <title>Roll</title> <property>/orientation/roll-deg</property> </entry> <entry n="6"> <enabled>true</enabled> <title>Pitch</title> <property>/orientation/pitch-deg</property> </entry> <entry n="7"> <enabled>true</enabled> <title>Heading</title> <property>/orientation/heading-deg</property> </entry> <entry n="8"> <enabled>true</enabled> <title>airspeed</title> <property>/velocities/airspeed-kt</property> </entry> </log> </logging>
Then I created a small VB tool (Yeah, kalman filters in VB ;-) ) to filter the data. This is the result:
One thing you can see, is that if I calculate the centripetal acceleration, and substract it from my y and z acceleration, the resulting roll angle has a standard deviation of only 2 degrees. Not bad!
Files for you download pleasures:
After my experiments with the thermophile sensors, I decided to give Sparkfun ‘s 5DOF a try. Thermophile sensors worked well, but you need 6 sensors to be able to make nice turns. Then it becomes annoying and a lot heavier than this 5DOF module. It’s not that much cheaper eather (10$ per thermophile sensor, 110$ for the 5DOF).
Anyhow, after some troubles with the creation of the test board, it is finally done:
I also needed a new programmer to program the dsPic I’m planning to use. Ebay pointed me to a chinese ICD2 clone. Not bad :-)
Now the electronics side of the tests is finished, I can move on to the software side (Kalman filter!) which is more my cup of tea.
It’s main features are
- Build in GPS (antenna not included)
- IMU (accelerometer + gyro) in 2 dimensions (pitch and roll)
- PIC 18F ready to program :-)
Selling at less than 300$, it’s pretty cheap!
Some remarks and stuff I’d love to see changed in the next version:
- The GPS isn’t the best available. uBlox GPS’s are a lot better.
- Not really optimised for weight…
- I wonder how easy it is to add other sensors.
- Is a 18F PIC powerfull enough?
- Doesn’t take advantage of the new 2-axis gyroscope in 1 chip.
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)