Introduction:
The inherently unstable nature of quadrotors requires the control algorithms to have an accurate measurement of the quadrotor’s position at any given time. The autopilot is configured with three axis gyros and accelerometers to provide readings for position and orientation. Unfortunately the gyros lose accuracy over time and drift, and the accelerometers are ineffective in dynamic quadrotor maneuvers and can not be relied upon. State estimation or the algorithm to determine what the quadrotor is actually doing at any given time, becomes a vital component for quadrotor control.
There are many solutions for state estimation including euler estimation and Kalman filtering. Kalman filtering approximates the position of the quadrotor by correlating the state model of the actual controller and the readings from the sensors. Because the filter takes into account the accuracy of the sensor it can provide a minimal error solution for the exact position. We chose to implement a Kalman filter for our quadrotor state estimation.
Overview:
Kalman filtering is an advanced topic that is still the focus of many doctorate thesis. This is a basic overview of Kalman fitler mechanics to provide context for our matlab and C code implementations.
The Kalman filter has two parts, the Predict and the Update.
Predict: The Kalman filter implements a mathematical model of the controller so that it can simulate what the controller will do given a set of inputs. In this way the filter predicts the position of the quadrotor by the commands that the controller gives it. Unfourtuantely the mathematical model is not perfect and outside disturbances and non-linearities result in the mathematical model being in need of correction, this is where the upadate sequence comes to play.
Update: The gyroscopes, accelerometers, and especially the camera, update the mathematical model of the predict stage to correct the discrepancies not accounted for in the math. The beauty of the update is that the sensors are weighted with a bias of how trustworthy the sensors actually are. In the case of the accelerometers, their actual measurements are not very reliable in the application of the quadrotor and therefore the mathematical prediction is weighted greater than the sensor readings. By updating the mathematical model with the sensor readings, a very accurate model for the state estimation of the quadrotor is created.
Our Implementation:
We initially simulated our Kalman filter in Matlab. Because we wanted to implement the filter on the Rabbit microcontroller, we knew we needed as simple an implementation as we could get. Our matlab simulation files are given.
The Rabbit runs Dynamic C code and so our next task was to translate the matlab code into C code. We attempted to use the Mex functions native to matlab to test our c code in the matlab environment, but the translation of variables presented some challenges and we focused on direct implementation on the Rabbit before we finished the matlab / C code integration. Our mostly completed code is given here.
Finally we implemented the Kalman filter directly on the Rabbit. The Rabbit control loop runs at about 50 hz and with the addition of the constant iterations of Kalman predict functions and the floating point math associated with the update function, the Rabbit only ran at 2 hz. Knowing that we could not function at 2 hz, we scrapped the Kalman filter and implemented the Procerus estimation routine. Our code is available below.
If we had more time we feel that the Kalman filter would be implementable on the Rabbit if we were to change all the floating point multiplies to integer multiplies. The Rabbit does not have a floating point unit and therefore every floating point multiply is very costly to the speed of exectuion. An integer based multiply is easy for the rabbit and would speed the Kalman filter up possibly 30 times faster. Given the time necessary, we would have done the integer multiply conversion as our next step.
Although it was hard to test, the Kalman filter code appeared to be functional for opeation, if the Rabbit could have executed it faster.
