Tracking a Cannon Ball with a Kalman Filter

So far, we have been using linear Kalman filters to track reasonably simple problems. However, more complex problems require more complex Kalman filters. One of the primary limitations of the Kalman filter is that it is not capable of handling non-linear situations very well. In this example, we will be tracking a cannon ball.

In this problem we will be firing a cannon ball, and tracking the projectile as it travels through the air. We will assume no air resistance. The measurements will be coming from a radar 30,480m downrange of the cannon. The radar will measure a distance \(r\) to the cannon ball, as well as an angle \(\theta\).

The relationship between \(r\), \(\theta\), the position of the radar and the position of the cannon ball are given by

Where \(y_T\) is the vertical position of the cannon ball, \(x_T\) is the horizontal position of the cannon ball, \(y_T\) is the vertical position of the radar and \(x_R\) is the horizontal position of the radar.

Therefore, we also have

The state we will use will be

And the measurements will be

Let's go ahead and code up the system that will be simulating the cannon ball. Since we want the linear Kalman filter and the non-linear Kalman filter to be on equal footing, let's use the exact same function, and just add the functionality we need now. We'll start with a struct to hold the data we need.

            
struct Data {
    pub time: Vec<f64>,
    pub x: Vec<f64>,
    pub vx: Vec<f64>,
    pub y: Vec<f64>,
    pub vy: Vec<f64>,
    pub r_measurements: Vec<f64>,
    pub theta_measurements: Vec<f64>,
}
            
        

The Data struct is pretty self explanatory - it contains everything we need as a result of the cannon ball simulation.

Now, we need to actually produce the data. The big idea here is we will be working out the cannon ball's position "objectively" using SUVAT and then converting into the measurements \(r\) and \(\theta\). We will assume the cannon ball's starting position is at \(x=y=0\). Remember that the radar station is downrange - in other words, the cannon ball is shot in the direction of the radar station. At some point, the cannon ball will be above the radar station.

We make the following get_data() function to perform the simulation.

            
fn get_data() -> Data {
    let mut t = 0.0;
    let mut x = 0.0;
    let mut y = 0.0;
    let vx = INIT_VELOCITY * INIT_ANGLE.to_radians().sin();
    let mut vy = INIT_VELOCITY * INIT_ANGLE.to_radians().cos();

    let mut t_history = vec![];
    let mut x_history = vec![];
    let mut y_history = vec![];
    let mut vx_history = vec![];
    let mut vy_history = vec![];
    let mut theta_measurements = vec![];
    let mut r_measurements = vec![];

    let theta_normal = Normal::new(0.0, THETA_ERROR).unwrap();
    let r_normal = Normal::new(0.0, R_ERROR).unwrap();
    let mut rng = rand::thread_rng();

    while y >= 0.0 {
        t_history.push(t);
        x_history.push(x);
        y_history.push(y);
        vx_history.push(vx);
        vy_history.push(vy);

        theta_measurements.push(theta(x, y) + theta_normal.sample(&mut rng));
        r_measurements.push(r(x, y) + r_normal.sample(&mut rng));

        // y direction
        let tmp_vy = vy - G * TS;

        y = y + 0.5 * (vy + tmp_vy) * TS;

        vy = tmp_vy;

        // x direction
        x = x + vx * TS;

        // t
        t += TS;
    }

    return Data {
        time: t_history,
        x: x_history,
        vx: vx_history,
        y: y_history,
        vy: vy_history,
        r_measurements: r_measurements,
        theta_measurements: theta_measurements,
    };
}
            
        

There's a few constants in there, while you can change them as much as you like, I'll give the values that we will use for the rest of this article.

• INIT_VELOCITY: f64 = 915.0 (m/s)
• INIT_ANGLE: f64 = 45.0 (degrees)
• THETA_ERROR: f64 = 0.01 (radians)
• R_ERROR: f64 = 30_500.0 (m)
• TS: f64 = 0.1 (s)

Let's start now solving the equations needed to derive the Kalman filter that we will be using. We start with the state-space equations.

So we know the dynamics matrix

Under normal circumstances we would derive the fundamental matrix like so

However, in this case we're lucky since

So the approximation

Therefore, we can get an exact solution by

The continuous process noise matrix is given by

Where \(\Phi_s\) is the spectral density of white noise. As always we can derive the discrete process noise via

So we can simply solve like so

Notice how \(\mathbf{Q}_k\) is \(\mathbf{Q}_k\) from the constant velocity example, but it appears twice in the matrix? That's not a coincidence - here we have two independent constant velocity problems!

Now for the linearisation of the problem. Doing this is actually much easier than you might expect - we simply use a Jacobian matrix as \(\mathbf{H}\). The linearised measurement equation is given by

Using the fact that

We can derive

And using the fact that

We can derive

So, \(\mathbf{H}\) becomes

So, we now have everything to actually perform the filtering by solving the Riccati equations.

Recall that

We also define

And so

Now that we have all the necessary components, we can look at some results.

We can see that the Kalman filter handles the situation extremely well - the errors are very impressive. What's more impressive is the residuals are tiny in \(x\) and \(y\) - in some instances down from \(600m\) to \(10m\)!