Extended Kalman Filter Math for State Estimation

Code examples and math behind state estimation for aerospace

Unfortunately this is not about SpaceX rocket landings, but this is how SpaceX lands rockets. In order for software to command highly complex aerial maneuvers like the Falcon 9 landing, the vehicle first needs to know its own state.

So, have you ever wondered how aerospace vehicles know where they are in 3D space? This is called state estimation, and usually makes use of the Extended Kalman Filter for making sense of noisy sensor data.

Full Implementation

For a more detailed explanation of the Jacobians and Quaternions and the like, check out my full implementation of a state estimator in C++ here: https://github.com/arush/state-estimation

Detailed Math

Here are my personal notes explaining Extended Kalman Filter math. Below you’ll find a simplified version.

Background

MEMS based IMU sensors have completely revolutionized aerospace allowing spacecraft, satellites, aircraft and drones to track vehicle state in 3 dimensions. This gives rise to basic software commands to perform highly complex aerial maneuvers like the infamous Falcon 9 landing.

So, have you ever wondered how IMUs actually work?

In the early days

Before MEMS (Microelectromechanical Systems), IMUs would use physical flywheels like the one pictured from the Apollo mission. Due to the conservation of angular momentum, when the vehicle changes its attitude you can measure the change in the angle of the gimbal.

Probably the most famous IMU ever made

Vibrating Structures Gyroscopes (MEMS) and the Coriolis Effect

Instead of spinning flywheels and gimbals, MEMS sensors use tiny vibrating masses that experience a Coriolis Effect when experiencing an angular movement. The change in direction of the oscillation can be measured and therefore the body rate angle can be calculated.

Sensors in a modern IMU

Most modern IMUs that track 6DoF (six degrees of freedom) actually have:

• 3 rate gyroscopes, one for each axis, and
• 3 accelerometers — also one for each axis
• temperature sensor to help correct the bias from each of the other sensors
• magnetometer
• barometer

All these measurements get fused in the onboard IMU processor which outputs the measurements for your flight controller to make use of.

The 12 Dimensions of an Aircraft’s State Vector

An IMU is used to track the state of a vehicle, and for an aircraft with 6DoF there are 12 values that make up the state vector for the vehicle:

1. 3D position: x,y,z
2. 3D translational velocity: x_dot, y_dot, z_dot
3. 3D orientation: ϕ,θ,ψ
4. 3D angular velocity: p,q,r

Integrate the derivatives at time (t) to get position and orientation

Groups #2 and #4 are the first-order derivatives of #1 and #3. Velocities track the rates of change and can be integrated to find the position and orientation. Since we know rate gyroscopes measure angular velocity (#4), accelerometers measure linear acceleration (second-order derivative of position), and we know the time of the measurements, we can integrate the derivatives to find orientation, translational velocity and then position.

But we need to account for noise.

Measurement Model for a Sensor

The measurements that the sensors spit out are unlikely to be the actual values because no sensors are perfect. We need to come up with a measurement model to handle the noise.

Rate Gyroscope example

For example for the gyroscopes, once we have a measurement model we can

1. estimate the body rates and then
2. integrate to get the actual attitude

This is an example of a measurement model for a gyroscope:

• eta (noise) changes for every measurement.
• bias (b) is constant for at least some amount of time (e.g. dependent on temperature).
• omega (true angular velocity) is unknown and is disturbed by the bias and some zero-mean gaussian noise, eta.

Filter the Noise with Bayesian Probability

Since noise is a gaussian distribution of known mean (zero) and covariance (you can find sigma on the spec sheet of the sensor), we can filter the noise with filtering algorithms like EKF and its variants which are based on Bayesian probability. Bayesian probability determines what is likely to be true based on past information.

Here’s a great resource for learning Kalman Filters and many other algorithms interactively with Jupyter Notebooks.

Measure the Angle and Uncertainty Over time

Now we have a way of estimating the probability of the true value given noisy measurements, we can use the above measurement to update the angle theta over timestep t like so:

For this equation to be useful we need to have recorded some initial angle theta_0 at timestep_0.

This is a discrete approximation for computing the integral so there would be some epsilon error added on but let’s ignore that for now.

So in your autopilot you might have some angle calculator where you’re tracking the measured angle over time:

def measured_angle(measured_omega, timestep):
  return last_measured_angle + measured_omega*timestep

Navigation by Dead Reckoning is Susceptible to Drift

Navigation that is dependent solely on inertial measurements like the above is called navigation by dead reckoning and is highly susceptible to drift even if you know the uncertainty of the measurement. Drift accumulates and gets worse over longer periods of time which is why these measurements need to be corrected every so often.

Sensor Fusion

This is why we need other sensors like GPS which is much slower but is not susceptible to drift. We fuse the data from IMU together with the GPS on a lower refresh rate, for example 10Hz, whereas the IMU measurements may come at 200Hz.

Here’s that exact example from my code:

Complimentary Filter

An EKF is not always necessary. A complimentary filter is much simpler and easier to compute. It is just a weighting of the two sensors, weighted based on how much you trust the error of each measurement.

This is an example of a complimentary filter for attitude using roll and pitch IMU measurements.

I chose to use Quaternions here to avoid gimbal lock and its a nice clean way to get the roll and pitch angles from IMU body rates

// set quaternion with current estimated euler angles
Quaternion<float> qt = qt.FromEuler123_RPY(rollEst, pitchEst, ekfState(6));

// integrate body rate which basically multiplies pqr measurements with timestep
// projects the quaternion forward in time, doesn't change its type, i.e. still a Quaternion<float>
qt.IntegrateBodyRate((V3D)gyro, (double)dtIMU);

// back to euler
V3D predictedRPY = qt.ToEulerRPY();
float predictedRoll = predictedRPY.x;
float predictedPitch = predictedRPY.y;
ekfState(6) = (float)predictedRPY.z; // yaw

EKF Predict

In every EKF there is first a predict() step when you propagate measurements forward in time to estimate where your next measurement is likely to be.

As mentioned above, we use the state vector and integrate linear acceleration to predict velocity and velocity to predict position. It looks something like this:

predictedState(0) += predictedState(3) * dt;
predictedState(1) += predictedState(4) * dt;
predictedState(2) += predictedState(5) * dt;

// attitude.Rotate_BtoI(<V3F>) to rotate a vector from body frame to inertial frame
const V3F acc_inertial = attitude.Rotate_BtoI(accel) - V3F(0.0F, 0.0F, CONST_GRAVITY);

// predict x_dot, y_dot, z_dot
predictedState(3) += acc_inertial.x * dt;
predictedState(4) += acc_inertial.y * dt;
predictedState(5) += acc_inertial.z * dt;

EKF Update

In the update() step we have to calculate the Jacobian every time. This is why EKFs can become compute intensive, as the larger your state vector, the larger your Jacobian matrix and subsequent calculations. The Jacobian is a way of describing the rate of change of each state vector with respect to the others. Think of it like a kind of multi-dimensional derivative.

First we compute Rgb’ using the below equation

In code that looks like this:

float theta = pitch;
float phi = roll;
float psi = yaw;

RbgPrime(0,0) = -cos(theta) * sin(psi);
RbgPrime(0,1) = -sin(phi) * sin(theta) * sin(psi) - cos(phi) * cos(psi);
RbgPrime(0,2) = -cos(phi) * sin(theta) * sin(psi) + sin(phi) * cos(psi);

RbgPrime(1,0) = cos(theta) * cos(psi);
RbgPrime(1,1) = sin(phi) * sin(theta) * cos(psi) - cos(phi) * sin(psi);
RbgPrime(1,2) = cos(phi) * sin(theta) * cos(psi) + sin(phi) * sin(psi);

RbgPrime(2,0) = 0;
RbgPrime(2,1) = 0;
RbgPrime(2,2) = 0;

Then we plug those values into the Jacobian matrix:

MatrixXf gPrime(QUAD_EKF_NUM_STATES, QUAD_EKF_NUM_STATES);
gPrime.setIdentity();

gPrime(0, 3) = dt;
gPrime(1, 4) = dt;
gPrime(2, 5) = dt;

// control input is just acceleration
gPrime(3, 6) = (RbgPrime(0) * accel).sum() * dt;
gPrime(4, 6) = (RbgPrime(1) * accel).sum() * dt;
gPrime(5, 6) = (RbgPrime(2) * accel).sum() * dt;

ekfCov = gPrime * ekfCov * gPrime.transpose() + Q;

This gives us a more accurate projection of error as time moves forward, watch how the error bounds move in sync with the data:

Magentometer Update

The magnetometer math is much simpler.

Which in code is:

hPrime(6) = 1;

// zFromX means estimated yaw (z) from state vector X
zFromX(0) = ekfState(6);

float diff = magYaw - zFromX(0);

// normalize
if (diff > F_PI) zFromX(0) += 2.f*F_PI;
if (diff < -F_PI) zFromX(0) -= 2.f*F_PI;

Update(z, hPrime, R_Mag, zFromX);

GPS Update

And finally, this is the formula for the GPS update step:

// The measurement z for the GPS is [x, y, z, x_dot, y_dot, z_dot] and the measurement model is h(xt) = [xt.x, xt.y xt.z, xt.x_dot, xt.y_dot, xt.z_dot].
hPrime(0, 0) = 1;
hPrime(1, 1) = 1;
hPrime(2, 2) = 1;
hPrime(3, 3) = 1;
hPrime(4, 4) = 1;
hPrime(5, 5) = 1;

zFromX(0) = ekfState(0);
zFromX(1) = ekfState(1);
zFromX(2) = ekfState(2);
zFromX(3) = ekfState(3);
zFromX(4) = ekfState(4);
zFromX(5) = ekfState(5);

Update(z, hPrime, R_GPS, zFromX);

And that rounds out all our estimation for all sensors!