Subscribe to The life of an Aerospace Engineer

## A short glance at (Extended) Kalman filtering

In my other blog post I mainly talked about the steps to implement a NDI controller. One of the assumptions I made there was that I had access to all states and that there was no noise in the sensors and the system (in short: I have an optimal system). So how do we overcome this? This is done by System identification.

System identification is not only an important in aerospace engineering, but also in control other processes. As sensors could be biased and the process itself contain noises, it is essential to smooth out the data while not destroying the general trend of it.
In general this is not enough, because smoothing out the data does not automatically mean that the states are reconstructed. This is where the concept of sensor fusion kicks in. By using multiple sensors (like accelerometers or GPS receivers) one can estimate the original state. As observed states are generally less than the original states (you don't have the place and the money to put in a sensor for each state) and are generally a manipulation of states, you need to have a proper algorithm which does not only cope with the noisy inputs, but also deals with estimating the states from observed data from the instruments.

For this assignment I will be using the Extended Kalman Filter (EKF) and the Iterated Extended Kalman Filter (IEKF) and compare the performance with each other.
These filters are described by the following algorithm:

```
Extended Kalman filter

x_k1k1 = xk0 //The initial guess of the parameters (A m x 1 matrix)
p_k1k1 = pk0 //The initial variance matrix (A m x m matrix)
R = R0 //The variance of the observed variables. Given by a diagonal

for i =  1 to infinity
% Prediction phase. Predict the actual states and the observed states
x_kk1 = x_k1k1 + xdot * dt //I used Euler integration here. You're free to use whatever integration method you want
z_kk1 = ObserverEquations(x_kk1) //This predicts the observed states

% Predict the variance matrix to determine our gain. We use a discrete time form
[Phi, Gamma] = ComputeDiscreteStateMatrices(JacobianF, JacobianG, measuredInput, deltaT)

p_kk1 = Phi * p_k1k1 * Phi^T + Gamma * Q * Gamma^T

% Correction
Ve = JacobianH * p_kk1 * JacobianH^T + R

% Compute the Kalman gain
L = p_kk1 * JacobianH^T * Ve^-1

% Compute the variables for the next timestep
x_kk = x_kk1 + L * zMeasured(i) - z_kk1
p_kk = (I - L * JacobianH) * p_kk1 * (I - L * JacobianH^T( + L * R + L^T

x_k1k1 = x_kk;
p_k1k1 = p_kk;
end

```

So what is the main difference between the EKF and the IEKF? The difference is the correction step. In the EKF, the correction step is applied directly to the calculation, while in the IEKF the relative error is minimized which is done by:

```epsilon = //The error margin. Normally a very small number

eta_k = x_kk1;
eta_k1 = x_kk1 + L*((zMeasured^T - z_kk1) - JacobianH*(x_kk1-eta_k));

relativeError = norm(eta_k1 - eta_k) / norm (eta_k);

while( relativeError > epsilon)
eta_k = eta_k1;

JacobianH = subs(H, stateVector, eta_k^T);
Ve = (JacobianH * p_kk1 * JacobianH^T + R);

% Compute the Kalman gain
L = p_kk1 * JacobianH^T * inv(Ve);

eta_k1 = x_kk1 + L*((zm(i,:)/>^T - observerEquations(eta_k)) - JacobianH*(x_kk1-eta_k));

relativeError = norm(eta_k1 - eta_k) / norm (eta_k);
end

```

The main advantage of this is the stability of the algorithm. The EKF can become unstable if the correction step involves a big correction. However, although the IEKF minimizes the error, it comes with an additional cost of extra iterations. How many iterations these are is a subject for my university assignment.

Now this whole posts looks rather awkward with this whole formula/algorithm thing and to be honest... it is
Unless you see it in Matlab, this all looks like a whole chunk of letter in words. Thus in one of my future posts I'll explain how you can use the symbolic toolbox of Matlab to retrieve basic entities, because the last thing I want to do is calculating the Jacobian of a complex system by hand.

### 0 Comments On This Entry

There are no Trackbacks for this entry

S M T W T F S
123
45678910
11121314151617
18 19 2021222324
25262728