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.
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.
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.
No comments:
Post a Comment