In my grand plan of things, I want to infer both the attitude and location of the plane. Location is somewhat easy to figure out thanks to GPS, but attitude is really a problem. Now, you might think that using an accelerometer or an inclinometer would be the solution. Unfortunately this is not the case, and the reason for this is very closely related to the so-called
pendulum rocket fallacy.
The fact that accelerometers can be used to infer the orientation of a cell phone follows directly from the fact, that you can (for the most part) assume that the phone is held stationary in a users hand. The moment this assumption is no longer true, you can't directly measure the direction of gravity. To me this was quite a difficult thing to understand. My intuition originally was that you could always measure gravity as you did in the cell phone. It wasn't until later, when I did a simulation of the attitude measurements, that I realized the acceleration measurements didn't give any information on the direction of gravity. Anyway, if you don't believe me on this, there are some
good explanations out there.
Okay. So how do you know which way is up? Very few methods allow you to measure your absolute attitude directly. Many of the methods give you only a part of your attitude and you have to combine many methods, to get your absolute attitude (roll, yaw, pitch). An incomplete list of possible methods:
- Guide stars. This is the only method I can think of right now which will actually give your absolute orientation. It only works at night. Requires you to know where you are and what time it is (GPS). Very difficult to measure.
- Horizon measurement. This could be either optical or thermal. Thermal works also during night, but not as well in the winter or in heavy overcast conditions. Can at most give you roll and pitch. Combined with GPS heading readings will give full absolute attitude. Sensors are quite expensive. Easy to measure.
- Earth's magnetic field measurements. Gives two components of the attitude. On the magnetic poles these components will correspond with roll and pitch. Combined with suitable other means of attitude inference can give full absolute attitude. Easy to measure.
- Polarization of the sky. Can be used to infer the components of the attitude, if you know where you are and what time it is (GPS). Combined with e.g. magnetic field measurements can give absolute attitude. Difficult to measure.
- Gyroscopes. Gyroscopes can be used to measure the angular velocity at which the plane is rotating. Hence they give you the changing rate of attitude. If you knew your attitude at start, and you had a perfect gyroscope, you could track your changes in attitude and thus know your current orientation. Unfortunately, there is no such thing as a perfect gyroscope (plus you'll have integration error anyway) and you end up with a drift. However, the drift can be quite low even with cheap solid state gyroscopes (in the order of a fraction of a degree per minute). Combined with e.g. magnetic field measurements can give absolute attitude (with a caveat). Easy to measure.
I want to only do easy stuff, so I can only really consider horizon measurement, magnetic field measurement and gyroscopes. Horizon measurements are a bit on the expensive side, and they also have problems during overcast conditions and winter (which sums up to about 90% of the time in Finland), so I'll have to go with magnetic fields and gyroscopes.
As luck would have it, some nice Chinese folks are selling
small IMU boards on ebay. They have everything I need for attitude measurement, as well as an accelerometer :-).
Let's begin with the first thing. How to represent the attitude of the plane? The first thing that comes to mind is the classic roll, yaw, pitch formulation, also known as Tait-Bryan angles, or improper Euler angles. This formulation is ultimately the form I would like to look at the attitude, so it would make sense to store them internally in this form as well. This formulation however has a nasty feature known as
gimbal locking. Also, the differential of the rotation has a form, which is heavy on computation (a lot of trigonometric functions, that would need to be evaluated at each time step). So this is not the way to go. I hate to say it (because I've been against the idea in the past), but a better solution is to use
Euler rotation parameters, also known as unit quaternions. This formulation is minimal on computational complexity and it easily converts to Tait-Bryan angles when needed. See
these lecture notes (starting from about lecture 7) for a very good tutorial on different rotation formalisms and their uses. Wikipedia also has
an article about this, but it is not as good in my opinion.
The most important reason for using the quaternion formulation of rotations is the absolute simplicity of the rotation time derivative. It is given simply as
,
where q is the rotation quaternion, \omega_x, \omega_y and \omega_z are angular velocities, which are measured by the gyroscope and the \otimes is the quaternion product. Using explicit Euler integration, a pseudo code implementation of a rotation update given the instantaneous angular velocity measurement (wx, wy, wz) is
updaterotation(wx,wy,wz) { /* old rotation defined by rqs, rqx, rqy, rqz length of time step dt */ rqs_up=rqs+dt*0.5f*(-rqx*wx-rqy*wy-rqz*wz); rqx_up=rqx+dt*0.5f*(+rqs*wx-rqz*wy+rqy*wz); rqy_up=rqy+dt*0.5f*(+rqz*wx+rqs*wy-rqx*wz); rqz_up=rqz+dt*0.5f*(-rqy*wx+rqx*wy+rqs*wz); /* normalize the quaternion */ rqinvnorm = 1.0f/sqrtf(rqx_up*rqx_up+ rqy_up*rqy_up+ rqz_up*rqz_up+ rqs_up*rqs_up); rqx=rqx_up*rqinvnorm; rqy=rqy_up*rqinvnorm; rqz=rqz_up*rqinvnorm; rqs=rqs_up*rqinvnorm;}
That takes care of integrating the angular velocity measurements to approximate our attitude. Now to get rid of the gyroscope drift by using measurements on the absolute direction of the magnetic field. As I only have very limited computational power on board, I had to come up with a really simple way to correct for the drift. I originally wanted to do something along the steps of the
extended Kalman filter (extended Kalman filter because it was the least computationally expensive non-linear flavour I could think of), but it would end up using quite a deal of memory since in addition to the current best estimate of the attitude, the covariance matrix of the estimate must also be stored. Most of all, it would also use quite a bit of CPU time, since for each update we need to solve a general 3 unknown linear equation. It probably would work though, but I didn't want to go through the hassle of testing how close it would be.
What I ended up doing can in some sense be explained as approximating the extended Kalman filter, by using a constant scalar covariance for the states. More precisely I did the following. When the plane is in its starting position and we know its attitude, we take a measurement of the magnetic field. This magnetic field direction, as well as the initial attitude, are stored. In the future, since we know what the magnetic field was at the initial attitude, we can compute what the magnetic field should be if we knew our attitude. Let's call the magnetic field measurement m, the actual attitude q and the function which connects the two h. Hence
However, we don't know our actual attitude, we know only an estimate q_est, so in general
As the equality should hold for the correct attitude, it would make sense to change the estimate so that the equality would hold. This however is not a good idea. The solution to the equation lacks uniqueness, and even if we had uniqueness, any measurement errors in the magnetic field would be amplified. In addition, the exact solution takes a lot of computational power, which we don't have. What we do instead, is that we simply take a small step in the negative gradient direction. If you don't know what that means, it just means that we change the estimate only by a little amount, but so that the new estimate is slightly better. The update is then
where \alpha is a relaxation parameter, which defines how long the steps should be that the algorithm takes. The lower the value, the slower the estimate converges. The higher it is, the more unstable the update is.
The algorithm to correct for a magnetic field measurement (mx, my, mz) in pseudocode
correctrotation(mx,my,mz) { /* old rotation estimate defined by rqs, rqx, rqy, rqz north direction in initial coordinates is gmx, gmy, gmz. */ /* compute difference between estimated and measured north direction */ mx_diff=mx-(+rqs*(gmx*rqs+gmy*rqz-gmz*rqy) +rqx*(gmx*rqx+gmy*rqy+gmz*rqz) -rqy*(gmz*rqs+gmx*rqy-gmy*rqx) +rqz*(gmy*rqs-gmx*rqz+gmz*rqx)); my_diff=my-(+rqs*(gmy*rqs-gmx*rqz+gmz*rqx) +rqx*(gmz*rqs+gmx*rqy-gmy*rqx) +rqy*(gmx*rqx+gmy*rqy+gmz*rqz) -rqz*(gmx*rqs+gmy*rqz-gmz*rqy)); mz_diff=mz-(+rqs*(gmz*rqs+gmx*rqy-gmy*rqx) -rqx*(gmy*rqs-gmx*rqz+gmz*rqx) +rqy*(gmx*rqs+gmy*rqz-gmz*rqy) +rqz*(gmx*rqx+gmy*rqy+gmz*rqz)); /* update rotation based on measured north direction error */ rqs_up=rqs+alpha*(+mx_diff*(+gmx*rqs+gmy*rqz-gmz*rqy) +my_diff*(-gmx*rqz+gmy*rqs+gmz*rqx) +mz_diff*(+gmx*rqy-gmy*rqx+gmz*rqs)); rqx_up=rqx+alpha*(+mx_diff*(+gmx*rqx+gmy*rqy+gmz*rqz) +my_diff*(+gmx*rqy-gmy*rqx+gmz*rqs) -mz_diff*(-gmx*rqz+gmy*rqs+gmz*rqx)); rqy_up=rqy+alpha*(-mx_diff*(+gmx*rqy-gmy*rqx+gmz*rqs) +my_diff*(+gmx*rqx+gmy*rqy+gmz*rqz) +mz_diff*(+gmx*rqs+gmy*rqz-gmz*rqy)); rqz_up=rqz+alpha*(+mx_diff*(-gmx*rqz+gmy*rqs+gmz*rqx) -my_diff*(+gmx*rqs+gmy*rqz-gmz*rqy) +mz_diff*(+gmx*rqx+gmy*rqy+gmz*rqz)); /* normalize the rotation */ rqinvnorm = 1.0f/sqrtf(rqx_up*rqx_up+ rqy_up*rqy_up+ rqz_up*rqz_up+ rqs_up*rqs_up); rqx=rqx_up*rqinvnorm; rqy=rqy_up*rqinvnorm; rqz=rqz_up*rqinvnorm; rqs=rqs_up*rqinvnorm;}These algorithms are easily light enough so that they can run on a low-end microcontroller, such as the ATmega328, over 200 times per second. In my tests, even after a long period (hours) of rotating the sensors around, the accumulated error is in the order of 5 degrees, which is about the accuracy of the magnetometer. However, I haven't as of yet done any in flight tests. I'm a bit worried of how much the large currents of the motor affects the magnetic field measurements.
