The Kalman filter basically consists of two stages.  In the first stage a mathematical state model is used to make a prediction about the system state.  In the next stage this state prediction is compared to measured state values.  The difference between the predicted and measured state is moderated based on estimated noise and error in the system and measurements, and a state estimation is output.  The output estimation is then used in conjuction with the mathematical state model to predict the future state during the next time update, and the cycle begins again.  

The figure above displays the equations and variables that make up a basic Kalman filter.  The meaning of most of the variables is fairly obvious.  It should be noted that the patameters Q and R represent the estimated expected error in the system model and in the measurements.  By changing these values, one can effectively "tune" the Kalman filter to obtain better results.  If, for example, the measurements of a system are considered to be very accurate, a small value for R would be used.  In this situation the Kalman filter output would follow the measure values more closely than the predicted state estimate.

The figure shown above is how the state transisition model takes the current input state and predicts a future state. These are some results from 1D generated accelerometer data correspoding to a 1 G acceleration followed by zero acceleration, and then followed by a -1G acceleration. This first example is with R = 0.3 and Q = 0.005

The above plots help to demonstrate the power of the kalman filter.  Even with fairly noisy accelerometer data we can achieve accurate estimations of velocity and position.  The second example also helps to demonstrate how Q and R affect the filter output.  Comparing the two different plots of acceleration, it can be seen that when R is smaller the Kalman output follows the measured acceleration follows more closely.

Figure:  Kalman State Model for Quaternions & Orientation

The figure above shows the state X and the model used to predict the future quaternion value.  Unlike the examples above for positioning, the relation between acceleration and positioning is a linear one and fairly easy to implement.  The relation between a quaternion and its derivative is more complicated, so we need to use the above model.  In this model q̇ represents the quaternion derivative, which is calculated using the current quaternion value and angular rotation rates.  For our system this rotation rates are supplied by the IMU gyros.  Below are some data plots showing the true, measured, and kalman filtered values for the four quaternion terms q1, q2, q3, and q4 corresponding to rotations about the three body frame axes.  For this example synthetic gyro and magnetometer data was created.  Noise was added to both data sets to create "measured" values.

As can be seen in the plots above, the Kalman filter is providing a better estimate of the system quaternion that defines orientation.  These results will be furthur improved once proper values for Q and R have been determined.  The team is currently in the process of finding these values, which involves tracking and combining sensor error values as the quaternions are calculated.

Even though the proper values of Q and R have yet to be determined, the estimated quaternion is still much improved over the measured values.  The videos below show the visualization of the sensor with measurement values and with filtered quaternion values.