Kalman Filter

What is a Kalman Filter?

A Kalman filter is an optimal estimator - ie infers parameters of interest from indirect, inaccurate and uncertain observations. It is recursive so that new measurements can be processed as they arrive.

If all noise is Gaussian, the Kalman filter minimises the mean square error of the estimated parameters.

For Shuttle Tracker we may use the Kalman Filter to minimize the mean square error of the location data that is received from the client. This is of great interest since the location data contains inaccurate and uncertain observations which we generally refer to as "noise". Using the Kalman filter we may model a shuttle using a state space and subsequently improve tracking performance.

Understanding the Kalman Filter

The Kalman Filter computations can be broken down into 5 equations that we must calculate.

1. State Extrapolation Equation – predicting or estimating the future state based on the known present estimation.
2. Covariance Extrapolation Equation – the measure of uncertainty in our prediction.
3. State Update Equation – estimating the current state based on the known past estimation and present measurement.
4. Covariance Update Equation – the measure of uncertainty in our estimation.
5. Kalman Gain Equation – required for computation of the update equations. The Kalman Gain is a "weighting" parameter for the measurement and the past estimations. It defines the weight of the past estimation and the weight of the measurement in estimating the current state.

Note that each of these equations may be represented in matrix notation which allows for faster computation run time which is of importance when modeling real-time systems. Therefore our first step will be to understand how we may break down each equation and convert it to its respective matrix representation.

Further note that this page is intended to be guide for implementing the Kalman Filter for Shuttle Tracker. The reader may wish to refer to the external resources link to get a better understanding of the underlying math for the Kalman Filter and how we reach the above 5 equations.

Past this point this guide will assume the general forms of the provided equations hold true, and we will derive the various components needed to utilize the general equations. We will leave it to the reader as an exercise to prove the general forms if they wish to do so.

1. State Extrapolation Equation – predicting or estimating the future state based on the known present estimation.

The general form of the State Extrapolation Equation is given as:

$$\hat{x}_{n+1,n} = F\hat{x}_{n,n}+Gu_n+w_n$$

Where:

• $\hat{x}_{n+1,n}$ is a predicted system state vector at time step $n+1$
• $x_{n,n}$ is an estimated system state vector at time step $n$
• $u_n$ is a control variable
• $w_n$ is the process noise
• $F$ is the state transition matrix
• $G$ is the control matrix

In the case of Shuttle Tracker there is no control input. Therefore we may simplify the general equation to our specific case as follows:

$$\hat{x}_{n+1,n} = F\hat{x}_{n,n}$$

Therefore let us do the following. First we will assign the system state, $s_n = x$, to represent the state of a shuttle on the X-Y plane where X will refer to longitude and Y will refer to latitude. Now let us assume that our system will assume constant acceleration over a time interval. This will allow us to use the basic Kinematic equations to model a step in our state from $n$ to $n+1$.

$$ s_n = \begin {bmatrix} x_{n} \\ \dot{x_{n}} \\ \ddot{x_{n}} \\ y_{n} \\ \dot{y_{n}} \\ \ddot{y_{n}} \end{bmatrix} $$

Note the following explanation of the system state matrix:

• $x_{n}$ and $y_{}$ inside the matrix respectively refers to longitude and latitude.
• $\dot{x_{n}}$ and $\dot{y_{n}}$ inside the matrix respectively refers to the velocity along the X-axis or along the longitude, and the velocity along the Y-axis or along the latitude.
• $\ddot{x_{n}}$ and $\ddot{y_{n}}$ inside the matrix respectively refers to the acceleration along the X-axis or along the longitude, and the acceleration along the Y-axis or along the latitude.

Now remember that we assume our system will be under constant acceleration during a given time interval. This allows to use the following Kinematic equations:

$$ \begin {aligned} x_{n+1} &= x_{n} + v_{n}\Delta t+\frac{1}{2}a\Delta t^2 \\ v_{n+1} &= v_{n} + a\Delta T \\ a_{n+1} &= a_{n} \end {aligned} $$

Therefore our system at state $s_{n+1}$ is equivalently represented as:

$$ s_{n+1,n} = \begin {bmatrix} x_{n,n} + \dot{x}{n,n}\Delta T + \frac{1}{2}\ddot{x}{n,n}\Delta t^2\ \dot{x}{n,n} + \ddot{x}{n,n}\Delta t \ \ddot{x}{n,n} \ y{n,n} + \dot{y}{n,n}\Delta T + \frac{1}{2}\ddot{y}{n,n}\Delta t^2\ \dot{y}{n,n} + \ddot{y}{n,n}\Delta t \ \ddot{y}_{n,n} \end{bmatrix} $$

Therefore we can rewrite the above to get the following matrix form:

$$ s_{n+1,n} = \begin {bmatrix} \hat{x}{n+1,n} \ \hat{\dot{x}}{n+1,n} \ \hat{\ddot{x}}{n+1,n} \ \hat{y}{n+1,n} \ \hat{\dot{y}}{n+1,n} \ \hat{\ddot{y}}{n+1,n} \end{bmatrix} = Fs_{n,n} = \begin {bmatrix} 1 & \Delta t & 0.5\Delta t^2 & 0 & 0 & 0 \ 0 & 1 & \Delta t & 0 & 0 & 0 \ 0 & 0 & 1 & 0 & 0 & 0 \ 0 & 0 & 0 & 1 & \Delta t & 0.5\Delta t^2 \ 0 & 0 & 0 & 0 & 1 & \Delta t \ 0 & 0 & 0 & 0 & 0 & 1 \end {bmatrix} \begin{bmatrix} \hat{x}{n,n} \ \hat{\dot{x}}{n,n} \ \hat{\ddot{x}}{n,n} \ \hat{y}{n,n} \ \hat{\dot{y}}{n,n} \ \hat{\ddot{y}}{n,n} \end{bmatrix} $$

This gives us our final matrix F represented as:

$$F = \begin {bmatrix} 1 & \Delta t & 0.5\Delta t^2 & 0 & 0 & 0 \\\ 0 & 1 & \Delta t & 0 & 0 & 0 \\\ 0 & 0 & 1 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 1 & \Delta t & 0.5\Delta t^2 \\\ 0 & 0 & 0 & 0 & 1 & \Delta t \\\ 0 & 0 & 0 & 0 & 0 & 1 \end {bmatrix}$$

2. Covariance Extrapolation Equation – the measure of uncertainty in our prediction.

The general form of the Covariance Extrapolation Equation is given as:

$$P_{n+1,n} = FP_{n,n}F^T+Q$$

Where:

• $P_{n,n}$ is the uncertainty (convariance) matrix of the current state estimation
• $P_{n+1,n}$ is the uncertainty (convariance) matrix of the next state estimation
• $F$ is the state transition matrix
• $Q$ is the process noise matrix

By definition of the covariance matrix we have:

$$P = \begin {bmatrix} p_x & p_{x\dot{x}} & p_{x\ddot{x}} & p_{xy} & p_{x\dot{y}} & p_{x\ddot{y}} \\\ p_{\dot{x}x} & p_{\dot{x}} & p_{\dot{x}\ddot{x}} & p_{\dot{x}y} & p_{\dot{x}\dot{y}} & p_{\dot{x}\ddot{y}} \\\ p_{\ddot{x}x} & p_{\ddot{x}\dot{x}} & p_{\ddot{x}} & p_{\ddot{x}y} & p_{\ddot{x}\dot{y}} & p_{\ddot{x}\ddot{y}} \\\ p_{yx} & p_{y\dot{x}} & p_{y\ddot{x}} & p_{y} & p_{y\dot{y}} & p_{y\ddot{y}} \\\ p_{\dot{y}x} & p_{\dot{y}\dot{x}} & p_{\dot{y}\ddot{x}} & p_{\dot{y}y} & p_{\dot{y}} & p_{\dot{y}\ddot{y}} \\\ p_{\ddot{y}x} & p_{\ddot{y}\dot{x}} & p_{\ddot{y}\ddot{x}} & p_{\ddot{y}y} & p_{\ddot{y}\dot{y}} & p_{\ddot{y}} \end {bmatrix}$$

Now, remember that along the main diagonal of a covariance matrix we have Cov(A,A) which is equivalent to the variance. Therefore the elements along the main diagonal of our covariance matrix, P, are the variances of the estimation given by:

• $p_x$ is the variance of the longitude coordinate position estimation
• $p_\dot{x}$ is the variance of the longitude coordinate velocity estimation
• $p_\ddot{x}$ is the variance of the longitude coordinate acceleration estimation
• $p_y$ is the variance of the latitude coordinate position estimation
• $p_\dot{y}$ is the variance of the latitude coordinate velocity estimation
• $p_\ddot{y}$ is the variance of the latitude coordinate acceleration estimation

We will now make one more assumption which is that the estimation errors in longitude, and latitude are not correlated so that the covariance between these terms can be set to zero.

This gives us our final matrix P represented as:

$$P = \begin {bmatrix} p_x & p_{x\dot{x}} & p_{x\ddot{x}} & 0 & 0 & 0 \\\ p_{\dot{x}x} & p_{\dot{x}} & p_{\dot{x}\ddot{x}} & 0 & 0 & 0 \\\ p_{\ddot{x}x} & p_{\ddot{x}\dot{x}} & p_{\ddot{x}} & 0 & 0 & 0 \\\ 0 & 0 & 0 & p_{y} & p_{y\dot{y}} & p_{y\ddot{y}} \\\ 0 & 0 & 0 & p_{\dot{y}y} & p_{\dot{y}} & p_{\dot{y}\ddot{y}} \\\ 0 & 0 & 0 & p_{\ddot{y}y} & p_{\ddot{y}\dot{y}} & p_{\ddot{y}} \end {bmatrix}$$

At this point we have , $F$, and $P$. Therefore we will now find $Q$, the process noise matrix.

This matrix Q, is also a Covariance Matrix.

We will assume a discrete noise model for Q which implies that the noise is different at each time sample but is constant between time samples.

Therefore our process noise matrix for our two-dimensional model will be represented as:

$$Q = \begin {bmatrix} \sigma^2_x & \sigma^2_{x\dot{x}} & \sigma^2_{x\ddot{x}} & \sigma^2_{xy} & \sigma^2_{x\dot{y}} & \sigma^2_{x\ddot{y}} \\\ \sigma^2_{\dot{x}x} & \sigma^2_{\dot{x}} & \sigma^2_{\dot{x}\ddot{x}} & \sigma^2_{\dot{x}y} & \sigma^2_{\dot{x}\dot{y}} & \sigma^2_{\dot{x}\ddot{y}} \\\ \sigma^2_{\ddot{x}x} & \sigma^2_{\ddot{x}\dot{x}} & \sigma^2_{\ddot{x}} & \sigma^2_{\ddot{x}y} & \sigma^2_{\ddot{x}\dot{y}} & \sigma^2_{\ddot{x}\ddot{y}} \\\ \sigma^2_{yx} & \sigma^2_{y\dot{x}} & \sigma^2_{y\ddot{x}} & \sigma^2_{y} & \sigma^2_{y\dot{y}} & \sigma^2_{y\ddot{y}} \\\ \sigma^2_{\dot{y}x} & \sigma^2_{\dot{y}\dot{x}} & \sigma^2_{\dot{y}\ddot{x}} & \sigma^2_{\dot{y}y} & \sigma^2_{\dot{y}} & \sigma^2_{\dot{y}\ddot{y}} \\\ \sigma^2_{\ddot{y}x} & \sigma^2_{\ddot{y}\dot{x}} & \sigma^2_{\ddot{y}\ddot{x}} & \sigma^2_{\ddot{y}y} & \sigma^2_{\ddot{y}\dot{y}} & \sigma^2_{\ddot{y}} \end {bmatrix}$$

Now similar to the Covariance Matrix. We will assume that the process noise for the longitude and latitude are not correlated so that the mutual terms may be set to 0.

Therefore our matrix Q is represented as:

$$Q = \begin {bmatrix} \sigma^2_x & \sigma^2_{x\dot{x}} & \sigma^2_{x\ddot{x}} & 0 & 0 & 0 \\\ \sigma^2_{\dot{x}x} & \sigma^2_{\dot{x}} & \sigma^2_{\dot{x}\ddot{x}} & 0 & 0 & 0 \\\ \sigma^2_{\ddot{x}x} & \sigma^2_{\ddot{x}\dot{x}} & \sigma^2_{\ddot{x}} & 0 & 0 & 0 \\\ 0 & 0 & 0 & \sigma^2_{y} & \sigma^2_{y\dot{y}} & \sigma^2_{y\ddot{y}} \\\ 0 & 0 & 0 & \sigma^2_{\dot{y}y} & \sigma^2_{\dot{y}} & \sigma^2_{\dot{y}\ddot{y}} \\\ 0 & 0 & 0 & \sigma^2_{\ddot{y}y} & \sigma^2_{\ddot{y}\dot{y}} & \sigma^2_{\ddot{y}} \end {bmatrix}$$

Now, importantly we will project the random variance in acceleration $\sigma^2_a$ on our dynamic model using, F, the state transition matrix.

For mathematical convenience let us first define a matrix, $Q_a$:

$$Q_a = \begin {bmatrix} 0 & 0 & 0 \\\ 0 & 0 & 0 \\\ 0 & 0 & 1 \end {bmatrix} \sigma_a^2$$

Therefore, we now must remember that Q represents the process noise matrix which is givne as follows:

$$Q = FQ_aF^T$$

Remember from Equation 1 that we have, *$F$, and that we have just derived $Q_a$. Therefore we can substitute and get:

$$F = \begin {bmatrix} 1 & \Delta t & 0.5\Delta t^2 \\\ 0 & 1 & \Delta t \\\ 0 & 0 & 1 \end {bmatrix}$$ $$= \begin {bmatrix} 1 & \Delta t & 0.5\Delta t^2 \\\ 0 & 1 & \Delta t \\\ 0 & 0 & 1 \end {bmatrix} \begin {bmatrix} 0 & 0 & 0 \\\ 0 & 0 & 0 \\\ 0 & 0 & 1 \end {bmatrix} \begin {bmatrix} 1 & 0 & 0 \\\ \Delta t & 1 & 0 \\\ \Delta t^2 & \Delta t & 1 \end {bmatrix}$$ $$= \begin {bmatrix} 0 & 0 & \frac{\Delta t^2}{2} \\\ 0 & 0 & \Delta t \\\ 0 & 0 & 1 \end {bmatrix} \begin {bmatrix} 1 & 0 & 0 \\\ \Delta t & 1 & 0 \\\ \Delta t^2 & \Delta t & 1 \end {bmatrix}$$ $$= \begin{bmatrix} \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} & \frac{\Delta t^2}{2} \\\ \frac{\Delta t^3}{3} & \Delta t^2 & \Delta t \\\ \frac{\Delta t^2}{2} & \Delta t & 1 \end {bmatrix}$$

Therefore our final Q matrix is given as:

$$Q = \begin {bmatrix} \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} & \frac{\Delta t^2}{2} & 0 & 0 & 0 \\\ \frac{\Delta t^3}{3} & \Delta t^2 & \Delta t & 0 & 0 & 0 \\\ \frac{\Delta t^2}{2} & \Delta t & 1 & 0 & 0 & 0 \\\ 0 & 0 & 0 & \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} & \frac{\Delta t^2}{2} \\\ 0 & 0 & 0 & \frac{\Delta t^3}{3} & \Delta t^2 & \Delta t \\\ 0 & 0 & 0 & \frac{\Delta t^2}{2} & \Delta t & 1 \end {bmatrix} \sigma_a^2$$

which has the following physical representations:

• $\Delta t$ is the time between successive measurements
• $\sigma_a^2$ is a random variance in acceleration.

Finally, we have all the pieces we need to construct the Covariance Extrapolation Equation.

Therefore we have our Covariance Extrapolation Equation given as:

$$P_{n+1,n} = \begin {bmatrix} p_{x_{n+1,n}} & p_{x\dot{x}_{n+1,n}} & p_{x\ddot{x}_{n+1,n}} & 0 & 0 & 0 \\\ p_{\dot{x}x_{n+1,n}} & p_{\dot{x}_{n+1,n}} & p_{\dot{x}\ddot{x}_{n+1,n}} & 0 & 0 & 0 \\\ p_{\ddot{x}x_{n+1,n}} & p_{\ddot{x}\dot{x}_{n+1,n}} & p_{\ddot{x}_{n+1,n}} & 0 & 0 & 0 \\\ 0 & 0 & 0 & p_{y_{n+1,n}} & p_{y\dot{y}_{n+1,n}} & p_{y\ddot{y}_{n+1,n}} \\\ 0 & 0 & 0 & p_{\dot{y}y_{n+1,n}} & p_{\dot{y}_{n+1,n}} & p_{\dot{y}\ddot{y}_{n+1,n}} \\\ 0 & 0 & 0 & p_{\ddot{y}y_{n+1,n}} & p_{\ddot{y}\dot{y}_{n+1,n}} & p_{\ddot{y}_{n+1,n}} \end {bmatrix} = FP_{n,n}F^T+Q$$ $$= \begin {bmatrix} 1 & \Delta t & 0.5\Delta t^2 & 0 & 0 & 0 \\\ 0 & 1 & \Delta t & 0 & 0 & 0 \\\ 0 & 0 & 1 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 1 & \Delta t & 0.5\Delta t^2 \\\ 0 & 0 & 0 & 0 & 1 & \Delta t \\\ 0 & 0 & 0 & 0 & 0 & 1 \end {bmatrix} \begin {bmatrix} p_{x_{n,n}} & p_{x\dot{x}_{n,n}} & p_{x\ddot{x}_{n,n}} & 0 & 0 & 0 \\\ p_{\dot{x}x_{n,n}} & p_{\dot{x}_{n,n}} & p_{\dot{x}\ddot{x}_{n,n}} & 0 & 0 & 0 \\\ p_{\ddot{x}x_{n,n}} & p_{\ddot{x}\dot{x}_{n,n}} & p_{\ddot{x}_{n,n}} & 0 & 0 & 0 \\\ 0 & 0 & 0 & p_{y_{n,n}} & p_{y\dot{y}_{n,n}} & p_{y\ddot{y}_{n,n}} \\\ 0 & 0 & 0 & p_{\dot{y}y_{n,n}} & p_{\dot{y}_{n,n}} & p_{\dot{y}\ddot{y}_{n,n}} \\\ 0 & 0 & 0 & p_{\ddot{y}y_{n,n}} & p_{\ddot{y}\dot{y}_{n,n}} & p_{\ddot{y}_{n,n}} \end {bmatrix} \begin {bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\\ \Delta t & 1 & 0 & 0 & 0 & 0 \\\ 0.5\Delta t^2 & \Delta t & 1 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 1 & 0 & 0\\\ 0 & 0 & 0 & 0 & \Delta t & 1 \\\ 0 & 0 & 0 & 0.5\Delta t^2 & \Delta t & 1 \end {bmatrix} + \begin {bmatrix} \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} & \frac{\Delta t^2}{2} & 0 & 0 & 0 \\\ \frac{\Delta t^3}{3} & \Delta t^2 & \Delta t & 0 & 0 & 0 \\\ \frac{\Delta t^2}{2} & \Delta t & 1 & 0 & 0 & 0 \\\ 0 & 0 & 0 & \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} & \frac{\Delta t^2}{2} \\\ 0 & 0 & 0 & \frac{\Delta t^3}{3} & \Delta t^2 & \Delta t \\\ 0 & 0 & 0 & \frac{\Delta t^2}{2} & \Delta t & 1 \end {bmatrix} \sigma_a^2$$

3 and 5. State Update Equation – estimating the current state based on the known past estimation and present measurement and Kalman Gain Equation.

The general form of the state update equation is given as:

$$\hat{s}_{n,n} = \hat{s}_{n,n-1} + K_n(z_{n} - H\hat{s}_{n,n-1})$$

where:

• $\hat{s}_{n,n}$ is the estimated system state vector at time step n.
• $\hat{s}_{n,n}$ is the predicted system state vector at time step n-1
• $K_n$ is the Kalman Gain
• $z_n$ is a measurement
• $H$ is the observation matrix

At this point I will note the importance of the Kalman Gain. Although we will not go into the proof behind the Kalman Gain, a simple intution for the Kalman Gain is as follows:

The Kalman Gain lies on a range from [0,1].

The closer to 0 the Kalman Gain is, the higher the measurement uncertainty, and the lower the estimate uncertainty. This implies we weigh the estimate uncertainty much higher, and the measurment uncertainty much lower.

The closer to 1 the Kalman gain is, the lower the measurement uncertainty, and the higher the estimate uncertainty. This implies we weigh the measurement uncertainty much higher, and the estimate uncertainty much lower.

If the Kalman Gain is 0.5 then the measurement uncertainty is equal to the estimate uncertainty.

Essentially the Kalman Gain defines how much a measurement changes the estimate. For Shuttle Tracker we may understand this to mean how much weight we put on the location data we receive from our users, or in other words how much we trust the location data to be accurate. If we believe the location data is highly accurate than a high Kalman Gain will influence our estimation algorithm from step n to step n+1 to be closer to the measured location data that is actually received. Conversely, if we believe the location data is highly inaccurate than a low Kalman Gain will influence our estimation algorithm from step n to step n+1 to be closer to the previous estimate at step n.

Therefore the Kalman Gain in matrix notation is given as:

$$K_n = P_{n,n-1}H^T(HP_{n,n-1}H^T+R_n)^{-1}$$

where:

• $K_n$ is the Kalman Gain
• $P_{n,n-1}$ is the previous estimate uncertainty (covariance matrix) of the current state which was predicted as the previous state
• $H$ is the observation matrix
• $R_n$ is measurement uncertainty otherwise known as the measurment noise of the covariance matrix

We have already derived $P_{n,n-1}$. Therefore we need the observation matrix, $H$ which is given as:

$$H = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 1 & 0 & 0 \end{bmatrix}$$

Note that the H matrix is "the observation model, which maps the true state space into the observed space" according to 2. What this means is that this matrix takes our estimate uncertainty matrix P and extracts the information we want from it through our observation matrix, $H$. Here our observation matrix $H$ will extract the following:

$$\begin{bmatrix} p_{x_{n,n-1}} \\\ p_{\dot{x}_{n,n-1}} \\\ p_{\ddot{x}_{n,n-1}} \end{bmatrix} \begin{bmatrix} p_{y_{n,n-1}} \\\ p_{\dot{y}_{n,n-1}} \\\ p_{\ddot{y}_{n,n-1}} \end{bmatrix}$$

which is the corresponding latitute, longitude, x,y velocities, and x,y acceleration data we are estimating for the shuttles.

Next, we need the measurement uncertainty, $R_n$ which is given as:

$$R_n = \begin{bmatrix} \sigma_{x_m}^2 & \sigma_{yx_m}^2\\\ \sigma_{xy_m}^2 & \sigma_{y_m}^2 \end{bmatrix}$$

where:

• the subscript $m$ is the masurement uncertainty

Again we assume that the $longitude$ and $latitude$ measurements are uncorrelated so that the errors are unrelated with each other. This gives us:

$$R_n = \begin{bmatrix} \sigma_{x_m}^2 & 0 \\\ 0 & \sigma_{y_m}^2 \end{bmatrix}$$

At this point I will note that this is simplistic and optimisitic. In many systems, including ours, the measurement uncertainty can differ between measurements because the systems that take the measurements can and will degrade. This is a complex problem with many parameters including things like, the angle between sensors, the target signal frequency, and others. For simplicity sake, we will assume that the measurement uncertainty is constant.

It may be the case that we will need a more complicated model for our $R_n$. In which case, the reader may find it helpful to start reading [2] which provides further reading into control theory using robust control for achieving stability in bounded modelling errors.

Therefore our Kalman Gain is given as:

$$K_n = \begin{bmatrix} K_{1,1_n} & K_{1,2_n} \\\ K_{2,1_n} & K_{2,2_n} \\\ K_{3,1_n} & K_{3,2_n} \\\ K_{4,1_n} & K_{4,2_n} \\\ K_{5,1_n} & K_{5,2_n} \\\ K_{6,1_n} & K_{6,2_n} \end{bmatrix} = P_{n,n-1}H^T(HP_{n,n-1}H^T+R_n)^{-1}$$ $$= \begin {bmatrix} p_{x_{n,n}} & p_{x\dot{x}_{n,n}} & p_{x\ddot{x}_{n,n}} & 0 & 0 & 0 \\\ p_{\dot{x}x_{n,n}} & p_{\dot{x}_{n,n}} & p_{\dot{x}\ddot{x}_{n,n}} & 0 & 0 & 0 \\\ p_{\ddot{x}x_{n,n}} & p_{\ddot{x}\dot{x}_{n,n}} & p_{\ddot{x}_{n,n}} & 0 & 0 & 0 \\\ 0 & 0 & 0 & p_{y_{n,n}} & p_{y\dot{y}_{n,n}} & p_{y\ddot{y}_{n,n}} \\\ 0 & 0 & 0 & p_{\dot{y}y_{n,n}} & p_{\dot{y}_{n,n}} & p_{\dot{y}\ddot{y}_{n,n}} \\\ 0 & 0 & 0 & p_{\ddot{y}y_{n,n}} & p_{\ddot{y}\dot{y}_{n,n}} & p_{\ddot{y}_{n,n}} \end {bmatrix} \begin{bmatrix} 1 & 0 \\\ 0 & 0 \\\ 0 & 0 \\\ 0 & 1 \\\ 0 & 0 \\\ 0 & 0 \end{bmatrix} \times$$ $$\left( \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 1 & 0 & 0 \end{bmatrix} \begin {bmatrix} p_{x_{n,n}} & p_{x\dot{x}_{n,n}} & p_{x\ddot{x}_{n,n}} & 0 & 0 & 0 \\\ p_{\dot{x}x_{n,n}} & p_{\dot{x}_{n,n}} & p_{\dot{x}\ddot{x}_{n,n}} & 0 & 0 & 0 \\\ p_{\ddot{x}x_{n,n}} & p_{\ddot{x}\dot{x}_{n,n}} & p_{\ddot{x}_{n,n}} & 0 & 0 & 0 \\\ 0 & 0 & 0 & p_{y_{n,n}} & p_{y\dot{y}_{n,n}} & p_{y\ddot{y}_{n,n}} \\\ 0 & 0 & 0 & p_{\dot{y}y_{n,n}} & p_{\dot{y}_{n,n}} & p_{\dot{y}\ddot{y}_{n,n}} \\\ 0 & 0 & 0 & p_{\ddot{y}y_{n,n}} & p_{\ddot{y}\dot{y}_{n,n}} & p_{\ddot{y}_{n,n}} \end {bmatrix} \begin{bmatrix} 1 & 0 \\\ 0 & 0 \\\ 0 & 0 \\\ 0 & 1 \\\ 0 & 0 \\\ 0 & 0 \end{bmatrix} + \begin{bmatrix} \sigma_{x_m}^2 & 0 \\\ 0 & \sigma_{y_m}^2 \end{bmatrix} \right)^{-1}$$

Therefore our final state update equation is given as:

$$\hat{s}_{n,n} = \begin {bmatrix} \hat{x}_{n,n} \\ \hat{\dot{x}}_{n,n} \\ \hat{\ddot{x}}_{n,n} \\\ \hat{y}_{n,n} \\ \hat{\dot{y}}_{n,n} \\ \hat{\ddot{y}}_{n,n} \end{bmatrix} = \hat{s}_{n,n-1} + K_n(z_{n} - H\hat{s}_{n,n-1})$$ $$= \begin {bmatrix} \hat{x}_{n,n-1} \\ \hat{\dot{x}}_{n,n-1} \\ \hat{\ddot{x}}_{n,n-1} \\\ \hat{y}_{n,n-1} \\ \hat{\dot{y}}_{n,n-1} \\ \hat{\ddot{y}}_{n,n-1} \end{bmatrix} + \begin{bmatrix} K_{1,1_n} & 0 \\\ K_{2,1_n} & 0 \\\ K_{3,1_n} & 0 \\\ 0 & K_{4,2_n} \\\ 0 & K_{5,2_n} \\\ 0 & K_{6,2_n} \end{bmatrix} \left( \begin{bmatrix} z_{long} z_{lat} \end{bmatrix} - \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 1 & 0 & 0 \end{bmatrix} \begin{bmatrix} \hat{x}_{n,n} \\ \hat{\dot{x}}_{n,n-1} \\ \hat{\ddot{x}}_{n,n-1} \\\ \hat{y}_{n} \\ \hat{\dot{y}}_{n,n-1} \\ \hat{\ddot{y}}_{n,n-1} \end{bmatrix} \right)^{-1}$$

4. Covariance Update Equation – the measure of uncertainty in our estimation.

The general form of the Covariance update equation is given as:

$$P_{n,n} = (I - K_nH)P_{n,n-1}(I-K_nH)^T+K_nR_nK_n^T$$

where:

• $I$ is the identity matrix
• $P_{n,n}$ is the estimate uncertainty (covariance matrix) of the current state
• $P_{n,n-1}$ is the previous estimate uncertainty (covariance matrix) of the current state which was predicted as the previous state
• $K_n$ is the Kalman Gain
• $H$ is the observation matrix
• $R_n$ is measurement uncertainty otherwise known as the measurment noise of the covariance matrix

We may note that these are all terms which have been derived in 3,5. Therefore we have all the building blocks we will need.

Therefore our final Covariance update equation is given as:

$$P_{n,n} = \begin {bmatrix} p_{x_{n,n}} & p_{x\dot{x}_{n,n}} & p_{x\ddot{x}_{n,n}} & 0 & 0 & 0 \\\ p_{\dot{x}x_{n,n}} & p_{\dot{x}_{n,n}} & p_{\dot{x}\ddot{x}_{n,n}} & 0 & 0 & 0 \\\ p_{\ddot{x}x_{n,n}} & p_{\ddot{x}\dot{x}_{n,n}} & p_{\ddot{x}_{n,n}} & 0 & 0 & 0 \\\ 0 & 0 & 0 & p_{y_{n,n}} & p_{y\dot{y}_{n,n}} & p_{y\ddot{y}_{n,n}} \\\ 0 & 0 & 0 & p_{\dot{y}y_{n,n}} & p_{\dot{y}_{n,n}} & p_{\dot{y}\ddot{y}_{n,n}} \\\ 0 & 0 & 0 & p_{\ddot{y}y_{n,n}} & p_{\ddot{y}\dot{y}_{n,n}} & p_{\ddot{y}_{n,n}} \end {bmatrix} = (I - K_nH)P_{n,n-1}(I-K_nH)^T+K_nR_nK_n^T$$ $$= \left( \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0\\\ 0 & 1 & 0 & 0 & 0 & 0\\\ 0 & 0 & 1 & 0 & 0 & 0\\\ 0 & 0 & 0 & 1 & 0 & 0\\\ 0 & 0 & 0 & 0 & 1 & 0\\\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} - \begin{bmatrix} K_{1,1_n} & 0 \\\ K_{2,1_n} & 0 \\\ K_{3,1_n} & 0 \\\ 0 & K_{4,2_n} \\\ 0 & K_{5,2_n} \\\ 0 & K_{6,2_n} \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 1 & 0 & 0 \end{bmatrix} \right) \begin {bmatrix} p_{x_{n,n-1}} & p_{x\dot{x}_{n,n-1}} & p_{x\ddot{x}_{n,n-1}} & 0 & 0 & 0 \\\ p_{\dot{x}x_{n,n-1}} & p_{\dot{x}_{n,n-1}} & p_{\dot{x}\ddot{x}_{n,n-1}} & 0 & 0 & 0 \\\ p_{\ddot{x}x_{n,n-1}} & p_{\ddot{x}\dot{x}_{n,n-1}} & p_{\ddot{x}_{n,n-1}} & 0 & 0 & 0 \\\ 0 & 0 & 0 & p_{y_{n,n-1}} & p_{y\dot{y}_{n,n-1}} & p_{y\ddot{y}_{n,n-1}} \\\ 0 & 0 & 0 & p_{\dot{y}y_{n,n-1}} & p_{\dot{y}_{n,n-1}} & p_{\dot{y}\ddot{y}_{n,n-1}} \\\ 0 & 0 & 0 & p_{\ddot{y}y_{n,n-1}} & p_{\ddot{y}\dot{y}_{n,n-1}} & p_{\ddot{y}_{n,n-1}} \end{bmatrix}$$ $$\times \left( \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0\\\ 0 & 1 & 0 & 0 & 0 & 0\\\ 0 & 0 & 1 & 0 & 0 & 0\\\ 0 & 0 & 0 & 1 & 0 & 0\\\ 0 & 0 & 0 & 0 & 1 & 0\\\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} - \begin{bmatrix} K_{1,1_n} & 0 \\\ K_{2,1_n} & 0 \\\ K_{3,1_n} & 0 \\\ 0 & K_{4,2_n} \\\ 0 & K_{5,2_n} \\\ 0 & K_{6,2_n} \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 1 & 0 & 0 \end{bmatrix} \right)^T + \begin{bmatrix} K_{1,1_n} & 0 \\\ K_{2,1_n} & 0 \\\ K_{3,1_n} & 0 \\\ 0 & K_{4,2_n} \\\ 0 & K_{5,2_n} \\\ 0 & K_{6,2_n} \end{bmatrix} \begin{bmatrix} \sigma_{x_m}^2 & 0 \\\ 0 & \sigma_{y_m}^2 \end{bmatrix} \begin{bmatrix} K_{1,1_n} & 0 \\\ K_{2,1_n} & 0 \\\ K_{3,1_n} & 0 \\\ 0 & K_{4,2_n} \\\ 0 & K_{5,2_n} \\\ 0 & K_{6,2_n} \end{bmatrix}^T$$

Some Final Thoughts

In this Wiki, we have been able to derive the classical Kalman Filter. Note that there are many moving parts, but the essence of what we want is to estimate the current state of a shuttle given some data. Therefore when actually implementing the Kalman Filter, the actual shuttle location that we will use comes from:

$$\hat{s}_{n,n} = \begin {bmatrix} \hat{x}_{n,n} \\ \hat{\dot{x}}_{n,n} \\ \hat{\ddot{x}}_{n,n} \\\ \hat{y}_{n,n} \\ \hat{\dot{y}}_{n,n} \\ \hat{\ddot{y}}_{n,n} \end{bmatrix} = \hat{s}_{n,n-1} + K_n(z_{n} - H\hat{s}_{n,n-1})$$ $$= \begin {bmatrix} \hat{x}_{n,n-1} \\ \hat{\dot{x}}_{n,n-1} \\ \hat{\ddot{x}}_{n,n-1} \\\ \hat{y}_{n,n-1} \\ \hat{\dot{y}}_{n,n-1} \\ \hat{\ddot{y}}_{n,n-1} \end{bmatrix} + \begin{bmatrix} K_{1,1_n} & 0 \\\ K_{2,1_n} & 0 \\\ K_{3,1_n} & 0 \\\ 0 & K_{4,2_n} \\\ 0 & K_{5,2_n} \\\ 0 & K_{6,2_n} \end{bmatrix} \left( \begin{bmatrix} z_{long} z_{lat} \end{bmatrix} - \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 1 & 0 & 0 \end{bmatrix} \begin{bmatrix} \hat{x}_{n,n} \\ \hat{\dot{x}}_{n,n-1} \\ \hat{\ddot{x}}_{n,n-1} \\\ \hat{y}_{n} \\ \hat{\dot{y}}_{n,n-1} \\ \hat{\ddot{y}}_{n,n-1} \end{bmatrix} \right)^{-1}$$

I will also note that it may not be accurate on the first pass. It may require dozens of iterations before converging on a suitable position uncertainty. It will be up to you the developer, to test various options for the following parameters: $R_n$, and $Q$, in order to tune the Kalman Filter to converge quickly without sacrificing on the uncertainty margins.

Finally, here are some closing thoughts which I hope may be of use during development.

The measurment periods as of 12/22/2022 for Shuttle Tracker will have $\Delta t$ = 1 minute or 60 seconds. Therefore the current state transition matrix is given as:

$$F = \begin{bmatrix} 1 & \Delta t & 0.5\Delta t^2 & 0 & 0 & 0 \\\ 0 & 1 & \Delta t & 0 & 0 & 0 \\\ 0 & 0 & 1 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 1 & \Delta t & 0.5\Delta t^2 \\\ 0 & 0 & 0 & 0 & 1 & \Delta t \\\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} = \begin {bmatrix} 1 & 60 & 30 & 0 & 0 & 0 \\\ 0 & 1 & 60 & 0 & 0 & 0 \\\ 0 & 0 & 1 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 1 & 60 & 30 \\\ 0 & 0 & 0 & 0 & 1 & 30 \\\ 0 & 0 & 0 & 0 & 0 & 1 \end {bmatrix}$$

Furthermore our process noise matrix is given as:

$$Q = \begin{bmatrix} \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} & \frac{\Delta t^2}{2} & 0 & 0 & 0 \\\ \frac{\Delta t^3}{3} & \Delta t^2 & \Delta t & 0 & 0 & 0 \\\ \frac{\Delta t^2}{2} & \Delta t & 1 & 0 & 0 & 0 \\\ 0 & 0 & 0 & \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} & \frac{\Delta t^2}{2} \\\ 0 & 0 & 0 & \frac{\Delta t^3}{3} & \Delta t^2 & \Delta t \\\ 0 & 0 & 0 & \frac{\Delta t^2}{2} & \Delta t & 1 \end {bmatrix} \sigma_a^2 = \begin{bmatrix} 3240000 & 108000 & 1800 & 0 & 0 & 0 \\\ 108000 & 3600 & 60 & 0 & 0 & 0 \\\ 1800 & 60 & 1 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 3240000 & 108000 & 1800 \\\ 0 & 0 & 0 & 108000 & 3600 & 60 \\\ 0 & 0 & 0 & 1800 & 60 & 1 \end {bmatrix} \sigma_a^2$$

Finally, the random acceleration standard deviation: $\sigma_a$ and $\sigma_{x_m}$ and $\sigma_{y_m}$ are values which we may tune. In order to find the values which produce a good $R_n$ and low error will require testing with various values for each combination of $\sigma$, and in particular $\sigma_a$ since we will note that is very improbable that the shuttles are traveling at a constant acceleration.

Some starter values which we may begin testing from which were found in [3]. are:

• $\sigma_a = 0.2m/s$
• $\sigma_{x_m} = \sigma_{y_m} = 3m$

Note that although our x, and y represent longitude and latitude, the $R_n$ is a general matrix for measurement uncertainty and does not need to be in terms of longitude or latitutde.

Links to External Resources:

• [1]. Understanding the Covariance Matrix.
• [2]. Kalman Filter Wikiwand
• [3]. Introduction to the Kalman Filter