Python - Kalman Filter (Unscented) not working corretly

Question

I am trying to implement a Kalman Filter which is used to filter calculated values of Speed, Heading, Acceleration and Yaw Rate. I also use the Kalman Filter to predict future values and use those to calculate future geographical positions of an object.

It seems that the Filter is resetting the predicted values to 0. Currently, my next state update is always maintained (nv = v, na = a, etc..), but if I change to nv = v + a * dt I get similar behaviour, values are very close to zero (~ e-14).

Main code:

# Initialize or retrieve the UKF instance
ukf = get_ukf(station_id)
# Update UKF with current raw measurements
ukf.update([speed, heading, acceleration, yaw_rate])

new_predictions = predict_future_positions(
    station_id,
    data[station_id].at[curr_index, 'latitude'], 
    data[station_id].at[curr_index, 'longitude']
)

Prediction code:

def fx(state, dt):
    """ Predict next state based on simple motion model assumptions """
    v, theta, a, psi_dot = state
    nv = v
    ntheta = theta          # Assuming constant heading for simplicity
    na = a                  # Assuming constant acceleration for simplicity
    npsi_dot = psi_dot      # Assuming constant yaw rate for simplicity
    return np.array([nv, ntheta, na, npsi_dot])


def hx(state):
    """ Measurement function returns the state itself, assumes you measure all states directly """
    return state


def initialize_ukf():
    """ Initialize and return a UKF for tracking speed, heading, acceleration, and yaw rate """
    sigmas = MerweScaledSigmaPoints(n=4, alpha=0.1, beta=2., kappa=1.)
    ukf = UKF(dim_x=4, dim_z=4, fx=fx, hx=hx, dt=1/30 , points=sigmas)
    ukf.x = np.array([0., 0., 0., 0.])  # initial state [speed, heading, acceleration, yaw_rate]
    ukf.P *= 10  # initial covariance
    ukf.R = np.eye(4) * 0.1  # measurement noise
    ukf.Q = np.eye(4) * 0.1  # process noise
    return ukf


def get_ukf(station_id):
    """ Retrieve or initialize a UKF for a specific station_id """
    if station_id not in ukfs:
        ukfs[station_id] = initialize_ukf()
    return ukfs[station_id]


def calculate_next_position(lat: float, lon: float, speed: float, heading: float, acceleration: float, interval) -> tuple:
    # Convert latitude, longitude and heading from degrees to radians
    lat_rad, lon_rad, heading_rad = map(math.radians, [lat, lon, heading])

    # Distance covered in the interval with the updated speed
    distance = speed * interval + 0.5 * acceleration * interval ** 2

    # Calculate new latitude and longitude in radians
    new_lat_rad = math.asin(math.sin(lat_rad) * math.cos(distance / EARTH_RADIUS) +
                            math.cos(lat_rad) * math.sin(distance / EARTH_RADIUS) * math.cos(heading_rad))
    new_lon_rad = lon_rad + math.atan2(math.sin(heading_rad) * math.sin(distance / EARTH_RADIUS) * math.cos(lat_rad),
                                       math.cos(distance / EARTH_RADIUS) - math.sin(lat_rad) * math.sin(new_lat_rad))

    # Convert new latitude and longitude from radians to degrees
    new_lat, new_lon = map(math.degrees, [new_lat_rad, new_lon_rad])

    return new_lat, new_lon


# def predict_future_positions(lat, lon, speed, heading, acceleration, curve_info, frequency=30, duration=5) -> list:
def predict_future_positions(station_id, lat, lon, frequency=30, duration=5) -> list:
    predicted_positions = []

    # Calculate the number of points and the duration of each interval
    points = frequency * duration
    interval_duration = 1 / frequency

    ukf = get_ukf(station_id)
    ukf.dt = interval_duration          # Set UKF's dt to the interval duration
    print(f"interval_duration: {ukf.dt}")

    for i in range(1, points + 1):        
        # Predict the next state using the UKF for the current interval
        ukf.predict()
        new_speed, new_heading, new_acceleration, new_yaw_rate = ukf.x
        print(f"Predicted: {new_speed}, {new_heading}, {new_acceleration}, {new_yaw_rate}")

        new_lat, new_lon = calculate_next_position(lat, lon, new_speed, new_heading, new_acceleration, interval_duration)
        predicted_positions.append({'lat': new_lat, 'lon': new_lon})
        lat, lon = new_lat, new_lon

    return predicted_positions

My output:

Speed: 10.254806246670183, Heading: 319.27693339350554, Acceleration: -0.23540827984615223, Yaw rate: -0.005145571837527121
interval_duration: 0.03333333333333333
Predicted: 10.166326468873194, 316.5221712417051, -0.2333771471468742, -0.005145058097190569
Predicted: 10.166326468873166, 316.52217124170784, -0.2333771471468764, -0.005145058097190472
...

Speed: 10.467646761934825, Heading: 319.23780992232264, Acceleration: 0.21254958566790286, Yaw rate: -0.03906999369681518
interval_duration: 0.03333333333333333
Predicted: 10.449913038293573, 319.0779853536578, 0.18630528143125535, -0.03707439752961145
Predicted: 10.449913038293431, 319.0779853536569, 0.1863052814312549, -0.037074397529612
...

Prediction accuracy: 0.10094114807863384 meters
Speed: 10.189528933971618, Heading: 319.25287353184626, Acceleration: -0.2778124154209819, Yaw rate: 0.015047067558968702
Predicted: 10.204898856119058, 319.2425502531232, -0.2504165308446913, 0.011970453237527212
Predicted: 10.204898856119286, 319.2425502531214, -0.25041653084469173, 0.011970453237527268
...

Prediction accuracy: 0.808794667925268 meters
Speed: 10.413539616314386, Heading: 322.54565576546065, Acceleration: 0.22384171917679088, Yaw rate: 3.2902986069174447
Predicted: 10.401223867329037, 322.3506784353949, 0.19584697089942216, 3.0967838480884247
Predicted: 10.401223867329037, 322.3506784353949, 0.19584697089942038, 3.0967838480884033
Predicted: 10.401223867329037, 322.3506784353949, 0.19584697089941927, 3.0967838480884176
...

Prediction accuracy: 0.5443148460430843 meters
Speed: 10.539412327559626, Heading: 323.6073064276114, Acceleration: 0.12570394929129958, Yaw rate: 1.0602272699128528
Predicted: 0.0, 0.0, 0.0, 0.0
Predicted: 0.0, 0.0, 0.0, 0.0
...

Prediction accuracy: 10.36880732693119 meters
Speed: 10.361936129429777, Heading: 335.33964303577073, Acceleration: -0.17724217428198916, Yaw rate: 11.71686610233176
...

Answer 1

(some caveats) I only have limited information about what you are trying to do. The code that you posted also does not reproduce your exact problem, so there may be other things going on in your code that I don't know about.

The kalman filter is useful when you have noisy dynamics and noisy measurements, and are trying to get your best estimate of the current state. In your filter you initialize the estimated state (which is different than the true state) as all zeros, then propagate it (which keeps all of the states constant in your case). I'm not surprised that the propagated state remains zero; that's the expected behavior. The biggest thing that you are missing is a call to ukf.update with a measurement of the true state. If you never update the state, then the filter will just keep propagating the initial state and never do any better than that.

Why don't you have latitude and longitude as part of your state?