Improved AHRS, part 1

While I'm still working on getting the autopilot working with my old navigation board, I'm already working on an improved version.

The old board included a 3 axis gyroscope, a 3 axis accelerometer, a 3 axis magnetometer and a GPS receiver. In addition to these, the new board will include a barometric pressure sensor and the GPS is upgraded to a model capable of 10 Hz output. The main reason for the update is however not the pressure sensor or faster GPS data rate, but the processor and the better sensor fusion algorithm it is (capable of) running.

In a previous post, I explained how the simple sensor fusion algorithm works on my current navigation board. It uses only the gyroscope and the magnetometer. The accelerometer is used only as an inclinometer when initializing the orientation of the plane prior to flight. In the post, I also explained that the accelerometer on its own doesn't provide any additional information of the attitude. This is still true. However, coupling the accelerometer history with measured position or velocity data does in fact give new information.

The reason for not doing sensor fusion with all sensors on the old navigation board was simply that the processor was too weak for handling it. With the new navigation board, the processor will be much more powerful and sensor fusion algorithm will be more complete. So far I've been developing on a Texas Instruments Tiva C Launchpad (previously known as the Stellaris Launchpad - a name change that has really caused some unnecessary confusion for me). I had that laying around, and it has also FPU as an added bonus. I'm not sure if I'll end up using the Tiva C series processors in the version that goes on a plane or if I'll use for instance an STM32 or an LPC. Anyway, I'm quite confident it'll be one of the Cortex-M3 or Cortex-M4 processors.

The blue board is a Chinese IMU card and the GPS module is a U-Blox (Fastrax) UP501. I quickly made a small board (TI calls them Boosterpacks) that fits the extension header of the Launchpad and then accepts the IMU and GPS. It also holds pull-up resistors for the I2C and a pull-down resistor.

The particular IMU board has a design flaw, so I don't recommend anyone getting one of this type. It turns out whoever designed the board didn't quite read the datasheet of the L3G4200D gyroscope. The gyro has two interrupt request pins, one which has everything to do with data being available (INT2) and another which is for detecting gestures (INT1). The designer of the board chose only to connect INT1 on the header (and to add insult to injury, they still connected an almost completely useless reset line of the barometric sensor). The lack of INT2 is a showstopper (even though the IC has a 32 measurement FIFO) when one wants to get maximal throughput. Luckily the interrupt pins are two adjacent pins on the IC package, so you can make a solder bridge to short them together. Then, to not short circuit the output drivers, you must configure the interrupts as open drain. However, very annoyingly (and not mentioned anywhere in the data sheet) when open drain interrupts are enabled on the L4G4200D, INT2 will pull high and INT1 will pull low. An open drain output pulling high is something I don't think I've ever come across before. It took me some time to realize the issue and change my pull-up resistor to a pull-down resistor. I really don't know why ST has decided to do that. With things the way they are, you cannot tie the interrupt lines together to produce a logical OR of the interrupts. If you tie them together and try to have both channels enabled, you'll end up having a short circuit regardless of whether the interrupts are configured as push-pull or open drain (and you can only configure both as push-pull or both as open drain). That kinda defeats the whole point of using open drain in the first place, unless you need to work around poorly designed Chinese boards and don't want to cut traces and solder in purple wire.

Enough with the rant, and on with the show. For the sensor fusion I was simply planning on using the extended Kalman filter. The Kalman filter is a causal state estimation filter, which uses a model to predict how the state of a system is changing in time, and uses measurement data to correct the estimated state. The idea behind the Kalman filter is simple. It keeps track of uncertainties of the state components and their mutual dependence. Assume you can measure B, but what you're really interested in is A. You know that B depends on A, and that there is uncertainty in A (and thus also on B). Now, you get a measurement of B. As you know what the connection between A and B is, you can correct the estimate of both B and A just by this measurement.

My old AHRS algorithm also started as the extended Kalman filter, but then got simplified to its current form by some additional assumptions. The simplifications actually made it equivalent to the gradient descent method. In this new system, however, I won't be making those simplifying assumptions. Like the old algorithm, the new one is also based on a kinematic model, by which I mean that flight dynamics are not considered in the AHRS. The control inputs of the model are the measured acceleration and measured angular velocity. These are used to update the estimated attitude as well as the possible states of velocity and position in global coordinates.

The estimate can be corrected through magnetic field, barometric pressure and/or GPS measurements. The magnetic field gives direct measurement of all but one attitude components. Barometric pressure can be used as an altimeter and thus can give the vertical component of the position. GPS gives horizontal components of the velocity and all three components of the position. The coordinate system I use is such that x=north, y=west, z=up. The attitude is most naturally stored as a rotation from local coordinates to global coordinates while using what is known as the quaternion parametrization.

There are a some choices of state configurations I've been thinking of, with different levels of complexity. The simplest (in some undefined sense) would be to use the horizontal GPS velocity measurements in addition to the magnetic field measurement. The state components would then be

The components q_s, q_x, q_y, q_z are scalar, x, y and z components, respectively, of the rotation in quaternion form. The components v_x and v_y are the components of velocity in x and y directions respectively, which are corrected through a GPS velocity measurement. This model does not use all available data, as GPS positions and barometer input are ignored, but it has less parameters that need tuning.

The next option would be to include the barometer measurements. The state components would then be

This now has the additional components v_z and r_z, which are the velocity and position in the z direction, i.e. altitude. The component r_z would be corrected through a barometric pressure measurement (GPS altitude is probably too inexact for this). This configuration adds new parameters to the system which require tuning. Also, care must be taken with the pressure measurement to get a meaningful altitude, as wind blowing on the sensor can skew the result.

The most complex option, which is capable of using all data given by the sensors, has the state vector

In addition to the position in the z direction, this configuration now also has r_x and r_y, the positions in x and y directions. These components may be corrected through GPS position measurements. As this configuration is the most complex, it also has the most parameters to tune, and most likely the most difficult to get working.

I think I'll gradually start from the least complex configuration, see if I can get it working, and then going from there. I was also planning on writing the actual algorithms here, but writing the math in blogger is just too tedious. Later I'll write a short paper documenting all the formulas and their derivations.

Successful flight with autopilot

Just a quick update. I finally got around to go flying again during the weekend. I had fixed some issues with the autopilot I discovered the last time I went flying (in August...). The autopilot now sets the heading in the following way:

1. GPS location is used to figure out heading to waypoint.
2. Waypoint heading is compared against GPS heading to give heading error.
3. Heading error is translated to target yaw rate.
4. Yaw rate is compared against measured yaw rate to give yaw rate error.
5. Yaw rate error is translated to roll angle.
6. Roll angle is compared against measured roll angle to give roll angle error.
7. Roll angle error is translated to aileron and rudder control.

Although the test was mostly successful and the autopilot could keep the plane under control (and even land the plane) the controller was only borderline stable. Horrible oscillations rise especially when the controller tries to track a heading. After looking at the recorded telemetry I've re-tuned the controllers. Hopefully next time will go smoother. Anyway, I'm happy that for the first time it actually works well enough to be useful.

See this video.

Receiver v3 sketching, part 2

After some work, I got the schematic of the new receiver revision ready. Or if not ready, then at least to a point, that I am quite happy with it. Check it out here.

The changes from the v2 schematic include obviously the 4051 multiplexer for the hardware PWM support, as well as the fact that all output connectors (servo, I2C and UART) are now on the single board. Also, I got rid of a lot of protection resistors, which I don't think were actually that relevant.

Receiver v3 sketching

In the end of my last post I mentioned that I was working on yet another revision of my receiver. The main reasons being the lack of hardware PWM support in the v2 revision as well as the difficulty of making the boards for a v2. Receiver v2 was designed for DIY manufacture of the PCB, which meant that it had to be possible to build without plated vias. This then meant that I couldn't have vias under components, and again that the boards had to be relatively large. To combat the boards becoming excessively big, I decided to split it to a two board design. This was a huge improvement on the v1 though, which was a three board design.

The biggest issue in the v3 is the hardware PWM. The processor used in v2 (the Atmel ATMEGA168) has only two hardware PWM channels capable of reasonable resolution servo pulses. This is the reason why v2 does the servo updates with bit-banging in software. Now, I could've just gone with some other processor which has at least eight reasonable PWM channels. However, as the ATMEGAx8 family of processors have worked well and since I'm quite familiar with them, I wanted to find a solution which allowed me to continue using them in the receiver.

As I am clearly exceeding the capabilities of the ATMEGAx8 processors, I do in fact need a new IC on the board. I was planning on using just a simple cheap analog multiplexer, namely the venerable 4051, which is a 1-to-8 bidirectional analog mux and costs around 0.20 €. As a servo pulse is high for a period of 1-2 ms every 20 ms and low the rest of the time, I can multiplex multiple servo channels on a single 16 bit hardware PWM channel. The idea is that while one channel is outputting its pulse, the other channels are at their low state. This is very similar to the pulse position modulation used in old analog RC transmitters and receivers.

Outputting 8 channels gives me 2.5 ms (= 20 ms/8) per channel. In other words, each channel has 2 ms time allocated for the actual pulse + 0.5 ms setup time. During the half millisecond we just need to run an ISR, which selects a new channel on the mux and loads the appropriate pulse width for it. It is a simple thing to do with a huge amount of time to do it. This means that the servo update requires almost no CPU time. Also, by adding a second mux and using the second 16 bit hardware PWM channel, I could trivially get up to 16 channels. 

The board is not really that big of an issue. Companies like ITead Studio provide PCB prototyping services, which give you 10 boards for 10 dollars. For that price it really doesn't make sense to do inferior quality boards at home. Of course I only need two or three boards, but it's still quite a reasonable price for those. The size limitation for the cheap boards is 5x5 cm, which should be easily achieved as the v2 boards already fit the limit. The multiplexer takes some additional space, but I'm confident the space saved by being able to use a more efficient board layout will more than pay it back.

I'm still alive!

It's been a long time since my last post. After becoming a father, it has been increasingly difficult to find the time to continue with my RC hobby. In fact, I've gone flying only a perhaps half a dozen times after my last post.

Plane lost

In my last post I talked about the possibility of implementing an autopilot in my receiver using the AHRS I've been working on. I implemented the autopilot as described in the last post, and tried it out for the first time in December. It didn't seem to work, as the plane would always just turn away and fly west when the autopilot was engaged. I was almost coming in for a landing, when disaster struck. I guess the melting snow shorted out the SPI lines, through which the processor and radio talk. This left me without control, which in turn caused the receiver to go into fail-safe. In this case it meant activating the autopilot. So the plane turned west, flew over a small forest and crashed somewhere. We searched for a couple of hours, but couldn't find the plane.

It wasn't until April this year, after the snow had melted, that some people walking in the woods found my plane and contacted me. The plane was in surprisingly good shape, considering it spent most of the winter outdoors. All the larger mechanical parts were intact, even the propeller wasn't damaged. Two servos were dead, both showing signs of overheating with melted plastic gears inside. I suspect the control surfaces got stuck against something, and the servos were stalling until the battery ran dry. Damage was also caused to the small 808 #11 keychain camera I had on board. It had its USB port ripped out, perhaps due a yank on the power cable as the plane hit a tree. The memory card in the camera was undamaged though. The only other casualty in the end was the flight battery.

I carefully washed and dehumidified all the electronics I had on board, and to my surprise all of  them came back to life. In the future, I plan to put a conformal coating on all of my electronics. This should not only protect against glitches due to water seeping in, but also prevent corrosion in the case the electronics do have to spend some time exposed to the elements.

I put the video from the camera on YouTube: two last minutes of the flight and whole flight with telemetry.

Air crash investigation

So the primary cause of the crash was the lost radio connection, and I could deduce that the problem was at the plane end. But why didn't the autopilot work? Why did the plane fly west? I suspected a programming error, so I started going through the autopilot code. However, I couldn't find any problems there. I soon realized that the telemetry I had recorded on my laptop before the radio failure, contained most of the parameters the autopilot based its decisions on. The only parameters it didn't include were the home coordinates the plane was flying toward.

I set up a simulation. I took a guess that the home coordinates were the first coordinates the telemetry contained. This is how the code should have worked. The first valid GPS fix after power on was supposed to be set as the home location. In the simulation it turned out, that the recorded roll angles were completely different from the target roll angles the simulated autopilot was producing. This was initially very strange.

When flying, I had noticed the autopilot caused the plane always to head west. So I tried simulating the autopilot output using home coordinates that were set a kilometer west from the flying site. This time the output matched the recorded values very well, and it became clear that the autopilot was working correctly, but it just didn't know where home was. I still didn't understand how the home coordinates could have been wrong. The code that set them seemed trivial, and it didn't seem very likely that the problem was there.

Since I didn't have a second set of hardware, it wasn't until I got the plane back, that I could connect a debugger to the system to figure out what was wrong with setting the home coordinates. It turned out that the problem was indeed not with the autopilot, but with the AHRS board. Due to a small bug, invalid GPS data was fed to the autopilot and labeled as valid. The autopilot would then sometimes take the invalid data and set it as the home coordinates. The plane would then of course try to fly to the fictional coordinates of something like one million degrees northern latitude and one billion degrees western longitude.

Now what?

The problems above are now all taken care of, and yesterday I finally flew the first quasi-successful test flight. It seems the autopilot still requires some tweaking of the controller parameters, as it sometimes goes into a catastrophic oscillation.

I'm also in the process of developing a new receiver. There are mainly two reasons for this:

1. Currently used software-only servo updates are inaccurate (3us resolution at best) and prone to jitter. The new version will have support for 8 hardware PWM channels at 0.13us resolution and (hopefully) negligible jitter. This will come at the expense of a multiplexer IC.
2. The current board is designed for DIY PCB manufacturing, which makes it quite large at 5cm x 2.5cm, and is a two-board design. The new version will be a small single board design, which I will order professionally made from ITead Studio.

I hope to write more on this new receiver quite soon.