Elev-8 flight control math description?
peterstern
Posts: 3
in Robotics
Is there a description or primer on the sensor data bit-banging and flight control math going on in quatimu.cpp?
I want to understand Jason Dorie's excellent work, but I need more than the code/comments/data sheets alone.
Not looking for anything general, unless its directly applicable to Jasons' math in quatimu.cpp.
Is there anything other than the code to help me figure this out?
I want to understand Jason Dorie's excellent work, but I need more than the code/comments/data sheets alone.
Not looking for anything general, unless its directly applicable to Jasons' math in quatimu.cpp.
Is there anything other than the code to help me figure this out?
Comments
I'm not aware of anything from Parallax (other than what you mentioned), but Jason is pretty active here and everyone else is pretty helpful as well.
Is there any part in particular you're wondering about?
http://forums.parallax.com/discussion/139895/gyroscope-l3g4200d
http://forums.parallax.com/discussion/163494/looking-for-ideas-fc-quadrover
The very short version is that it maintains an orientation estimate that is a quaternion. It is updated by rotating it with the gyro values, and slowly corrected so the 'down' direction of it aligns with the accelerometer vector.
There's also a "control" quaternion which is the currently desired orientation. That one is produced differently depending on the current control mode (manual or auto-level).
The difference between these two quaternions is computed, and that difference produces three numbers which are the amount of rotation required about each of the three control axis to align the orientation estimate with the desired control orientation. These numbers are fed into the PIDs that manage the roll/pitch/yaw motor outputs.
There's some extra stuff in the QuatIMU code that handles altitude estimation, and the newer versions of the code (not yet main-line) include compass lock (heading hold), which adds some complexity, but the overall process is similar.
Jason, we appreciate your lurking very much.
Happy New Year!
https://github.com/parallaxinc/Flight-Controller/blob/FloatStreamCompiler/FloatStreamCreator/functionstream.cpp
This code is parsed directly by a "compiler" I wrote that generates the F32 stream code, making it much simpler to iterate on (I used to do it by hand). The C version is still moderately dense code, but in general the formatting, comments, and higher-level nature of it make it much easier to read than the F32 token version.
Jason, thank you for your good explanation. I recently finished my own autopilot project, where I basically just reinvented the wheel it turns out. For me, reinvention was easier than "book learning."
I have a question maybe you can help me with. I was thinking about taking up the study of the Kalman filter. But right now my understanding is that there will be no real application to a project such as a GPS-less elev-8. If the sensor fusion here is done well and adequately handles the gyro drift, then there is absolutely nothing to be gained with a Kalman filter for a GPS-less elev-8 (that doesn't have any sort of navigation function). Right?
The reason I ask is because I don't see how a Kalman filter can in any way improve the software. In which case learning it would just be wasting my time. As far as I can tell (right now) the Kalman filter is really only good for one or two things.
That said, the Kalman filter is similar to a complimentary filter. It's more mathematically complex, but they essentially do the same thing, which is to fuse an estimated value with a measured value, with parameters for tuning the level of trust you have in each. With a complimentary filter, the math is quite simple:
Estimate = (Estimate * Trust) + (Measured * (1.0 - Trust))
This requires that the Estimate and Measured values are both expressed in the same units, and that the Trust value is a value between 0.0 and 1.0. (However 0.0 and 1.0 mean you're selecting one of the two inputs, not actually fusing both of them together)
This is almost exactly what the Elev8-FC is doing internally - I compute the amount of rotation required to align the current rotation estimate "up" vector with the current accelerometer vector, scale that rotation amount down and feed those values into the gyro update on the next iteration by just adding them to the gyro readings. I figure since I have to rotate the quaternion by the gyro rotation on the next update I might as well just feed the correction amounts in at the same time.
I haven't dug into the math of a Kalman filter, so I can't explain how they work, but they are mathematically more complex, and I believe that they can be tuned to adjust the noise rejection. A well tuned Kalman filter will supposedly be a little more accurate than a simple complimentary filter, but they are harder to tune and more difficult to implement.
Part of the difficulty with either of these approaches is that they are described using scalars. A rotation is a set of numbers that are all inter-related and have to be handled as a single unit, so any of these approaches have to be re-expressed to handle the rotation as a whole, not just a scalar quantity, and that trips up a lot of people.
Still not sure if I have my head around the idea though...
Too big to fit.
thanks a lot for the comments. I read through that rtf file when i first got the elev-8, but had forgotten about it. I am doing everything I can so I don't waste your time, brushing up on control theory, quaternions, Kalman filters, etc. Used to do this for a living but I am very rusty.
I won't bother you unless I am really stuck.
RE: kalman filters and their application to an elev-8: I think they might be useful to an elev-8 control problem anytime you can figure out an error relationship between sensor inputs, or between control (ie, motion) and sensor inputs. Even without GPS, there is a lot of potential sensor inputs and motion to apply a Kalman filter to. Some IMU chips even do correlation and filtering internally, like the MPU-6050. Would love to hear if anyone has any experience knowing when a Kalman filter is really necessary. In my limited experience, simpler solutions usually work fine, even when experts are telling me it will never work without a Kalman filter....
The fact that it's implemented on the Propeller has played a role in the direction of the code, because I have to keep it small and simple, and almost entirely free of conditional logic, at least in the IMU / orientation code. A lot of existing flight controllers are littered with special cases, but the FPU stream doesn't have space for much in the way of conditionals, so I had to come up with some semi-novel ways to handle the logical bits that are required (like not dividing by zero when you try to normalize a bad vector, or rejecting vectors that are outside of certain length restrictions).