The following pdf describes most of the theoretical approach behind the Extended Kalman Filter (EKF) used the INS/GPS implementation.

INSGPSAlg.pdf

# Complementary filter

CC3D use a simple complementary filter, to fuse CC3D's accelerometer and gyroscopes into an estimate of the orientation in space or attitude. This complementary filter however gets skewed by centripetal forces (aka when flying in circles for a longer time) and it can not calculate speed and position. It cannot even estimate which way CC3D is facing, since CC3D has no compass.

# State estimation with Extended Kalman Filter (EKF)

The sensors on any common Flight Controller cannot observe the system state directly. This means the flight computer cannot see the orientation, position and movement speed of the craft as is. Instead it has sensors similar to the human "inner ear" that provide it with rotation and acceleration measurements, a compass, a barometer, and possibly a global positioning system (GPS) from which that state information has to be computed.

The measurements taken by these sensors however are noisy, inaccurate and affected by temperature dependent offsets. Therefore a filter needs to be used to fuse all these sensors into a consistent view of the actual situation, the so called state estimation. This process is known as sensor fusion.

## State

Revolution boards runs an Extended Kalman Filter to compute a 13 dimensional system state, including a complete covariance in real time.

The following 13 state variables are computed:

1. Position in space - N (North) axis
2. Position in space - E (East) axis
3. Position in space - D (Down) axis
4. Movement velocity - N axis
5. Movement velocity - E axis
6. Movement velocity - D axis
7. Orientation in space - Quaternion - Q0
8. Orientation in space - Quaternion - Q1
9. Orientation in space - Quaternion - Q2
10. Orientation in space - Quaternion - Q3
11. Gyroscope drift bias - X axis
12. Gyroscope drift bias - Y axis
13. Gyroscope drift bias - Z axis

## Prediction model

The EKF runs iteratively at an update rate of 1000 updates per second (1kHz). It has a simple movement model of the craft, that simply computes the change in estimated velocity and orientation during the 1/1000 second based on the current measurement of the gyroscope (corrected by the estimated gyroscope drift bias) and accelerometers. The movement model takes the gravity vector into account to adjust the acceleration accordingly.

The following sensors are used for this:

• Accelerometer measurement - X axis
• Accelerometer measurement - Y axis
• Accelerometer measurement - Z axis
• Gyroscope measurement - X axis
• Gyroscope measurement - Y axis
• Gyroscope measurement - Z axis

This is called the prediction step. The new estimation then gets updated in a correction step.

## Correction step

The correction uses the following sensors to improve the state estimation:

In each step the following sensors can be measured to improve the estimation of the orientation in space.

• Magnetometer measurement - X axis
• Magnetometer measurement - Y axis
• Magnetometer measurement - Z axis

Other sensors have a slower update rate and are not available in every step but at a slower rate:

• Barometric altitude measurement (altitude in meters, corrected by a slowly adjusted pressure offset) - D axis
• GPS position (latitude/Longitude recomputed into NED coordinates relative to home location) - N axis
• GPS position (latitude/Longitude recomputed into NED coordinates relative to home location) - E axis
• GPS position (altitude) - D axis
• GPS velocity - N axis
• GPS velocity - E axis
• GPS velocity - D axis
• Airspeed (if available)

## Variances

An extended Kalman filter tracks not only the most likely system state in the way the complementary filter does, but also how different components of the state relate to each other, and how sure the filter is about each part of the state. This allows the EKF to adjust and improve all state variables from any single sensor measurement. The necessary relationship information is stored in the covariance matrix of the filter.

In every prediction step, the system overall variance increases, measuring the (un)certainty in the system. Every correction step usually decreases the overall uncertainty. However the sensor measurements themselves are not 100% certain, as each sensor has its own sensor variance. By definition the variance is simply the average error squared. For example, lets assume the GPS altitude is by average off by 3 meters, then this sensor's variance would be 3²=9m.

With most sensors (accelerometers, gyroscopes and magnetometers) the most obvious and easy to measure error is noise. This is a zero mean deviation, that can easily be estimated for example by the sensor noise calibration plugin in the GCS config widget.

However this easy to measure noise is not the only error, there might also be other errors such as magnetic disturbances in flight coming from the engines or non 100% correct calibration of sensors, leading to offset sensor readings and the like. Therefore it might be necessary to set the variance higher than the noise calibration suggests.