Python无迹卡尔曼滤波(UKF)异常：预测值归零问题排查

我正在用无迹卡尔曼滤波（UKF）对速度、航向、加速度和偏航率进行滤波，同时用它预测未来状态并计算目标的未来地理位置。但发现滤波器会将预测值重置为0——当前状态更新用的是保持原值的方式（比如nv = v、na = a），改成nv = v + a * dt后，预测值还是会趋近于0（约e-14量级）。

主代码 #

# 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']
)

预测逻辑代码 #

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

运行输出 #

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
...

内容的提问来源于Stack Exchange，提问作者André Clérigo