如何在PyKalman上优化扩展卡尔曼滤波？

Question

我想纠正一些预测的车辆变量:纬度、经度、速度、航向和加速度。在下面的代码中，有这些变量的值。共有20个样本，前10个样本是原始数据，最后10个样本是预测数据。我期望EKF去噪那些预测值，然而，计算值甚至远离原始值。

如何调优代码以获得实际的计算值？否则，是否有技术来修正预测的车辆坐标？

import numpy as np

np.set_printoptions(precision=6, suppress=True)

A is the identity matrix.
# A is sometimes F in the literature.
A_k_minus_1 = np.array([[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]])

# This is a vector with the number of elements equal to the number of states
process_noise_v_k_minus_1 = np.array([0.01, 0.01, 0.01, 0.01, 0.01])

# State model noise covariance matrix Q_k
# When Q is large, the Kalman Filter tracks large changes in
# the sensor measurements more closely than for smaller Q.
# Q is a square matrix that has the same number of rows as states.
Q_k = np.array([[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]])

# Measurement matrix H_k
# Used to convert the predicted state estimate at time k
# into predicted sensor measurements at time k.
# In this case, H will be the identity matrix since the
# estimated state maps directly to state measurements data
# H has the same number of rows as sensor measurements
# and same number of columns as states.
H_k = np.array([[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]])

# Sensor measurement noise covariance matrix R_k
# Has the same number of rows and columns as sensor measurements.
# If we are sure about the measurements, R will be near zero.
R_k = np.array([[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]])

# Sensor noise. This is a vector with the
# number of elements equal to the number of sensor measurements.
sensor_noise_w_k = np.array([0.01, 0.01, 0.01, 0.01, 0.01])

def ekf(z_k_observation_vector, state_estimate_k_minus_1,
        control_vector_k_minus_1, P_k_minus_1, dk):
    
    ######################### Predict #############################
    # Predict the state estimate at time k based on the state
    # estimate at time k-1 and the control input applied at time k-1.
    state_estimate_k = A_k_minus_1 @ (
        state_estimate_k_minus_1) +  (
                           control_vector_k_minus_1) + (
                           process_noise_v_k_minus_1)

    print(f'State Estimate Before EKF={state_estimate_k}')

    # Predict the state covariance estimate based on the previous
    # covariance and some noise
    P_k = A_k_minus_1 @ P_k_minus_1 @ A_k_minus_1.T + (
        Q_k)

    ################### Update (Correct) ##########################
    # Calculate the difference between the actual sensor measurements
    # at time k minus what the measurement model predicted
    # the sensor measurements would be for the current timestep k.
    measurement_residual_y_k = z_k_observation_vector - (
            (H_k @ state_estimate_k) + (sensor_noise_w_k))

    print(f'Observation={z_k_observation_vector}')

    # Calculate the measurement residual covariance
    S_k = H_k @ P_k @ H_k.T + R_k

    # Calculate the near-optimal Kalman gain
    # We use pseudoinverse since some of the matrices might be
    # non-square or singular.
    K_k = P_k @ H_k.T @ np.linalg.pinv(S_k)

    # Calculate an updated state estimate for time k
    state_estimate_k = state_estimate_k + (K_k @ measurement_residual_y_k)

    # Update the state covariance estimate for time k
    P_k = P_k - (K_k @ H_k @ P_k)

    # Print the best (near-optimal) estimate of the current state of the robot
    print(f'State Estimate After EKF={state_estimate_k}')

    # Return the updated state and covariance estimates
    return state_estimate_k, P_k


def main():
    # We start at time k=1
    k = 1

    # Time interval in seconds
    dk = 1

    # Create a list of sensor observations at successive timesteps
    # Each list within z_k is an observation vector.
   
      z_k = np.array([[41.3920877,  2.165009483,    11.32350106,    190.255528, 1.05357508],
                                [41.39198647,   2.16505734, 12.49313626,    148.3052463,    1.169635197],
                                [41.3919087,    2.165148001,    11.48702769,    139.2730885,    -1.006108567],
                                [41.39181576,   2.165256339,    13.72677596,    139.2730885,    2.239748268],
                                [41.39173316,   2.165357368,    12.46676741,    137.5553644,    -1.260008547],
                                [41.39166792,   2.165438147,    9.901313858,    137.5553644,    -2.565453555],
                                [41.39162805,   2.165487467,    6.047907658,    137.5827278,    -3.8534062],
                                [41.39161926,   2.165498327,    1.332407586,    137.5954214,    -4.715500072],
                                [41.39161926,   2.165498327,    0,  137.5954214,    -1.332407586],
                                [41.39161926,   2.165498327,    0,  137.5954214,    0],
                                [41.391619,  2.165498, -1.930117,  137.595421,  1.023591],
                                [41.391619,  2.165498, -1.710974,  137.595421,  2.246764],
                                [41.391619,  2.165498, - 0.375267,  137.595421,  3.334014],
                                [41.391619,  2.165498,  2.287329,  137.595421,  4.192694],
                                [41.391619,  2.165498,  6.132224,  137.595421,  4.717386],
                                [41.391619,  2.165498,  7.416489,  137.595421,  4.833027],
                                [41.391619,  2.165498,  7.555815,  137.595421,  4.833027],
                                [41.391619,  2.165498,  9.685403,  137.595421,  4.833027],
                                [41.391619,  2.165498,  9.696774,  137.595421,  4.833027],
                                [41.391619,  2.165498,  8.403750,  137.595421,  4.833027]])
    # The estimated state vector at time k-1 in the global reference frame.
    # [x_k_minus_1, y_k_minus_1, yaw_k_minus_1]
    # [meters, meters, radians]
    state_estimate_k_minus_1 = np.array([0.0, 0.0, 0.0, 0.0, 0.0])

    # The control input vector at time k-1 in the global reference frame.
    # [v, yaw_rate]
    # [meters/second, radians/second]
    # In the literature, this is commonly u.
    # Because there is no angular velocity and the robot begins at the
    # origin with a 0 radians yaw angle, this robot is traveling along
    # the positive x-axis in the global reference frame.
    control_vector_k_minus_1 = np.array([41, 2, 0.0, 0.0, 0.0])

    # State covariance matrix P_k_minus_1
    # This matrix has the same number of rows (and columns) as the
    # number of states. P is sometimes referred
    # to as Sigma in the literature. It represents an estimate of
    # the accuracy of the state estimate at time k made using the
    # state transition matrix. We start off with guessed values.
    P_k_minus_1 = np.array([[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]])
    # Start at k=1 and go through each of the 20 sensor observations,
    # one at a time.
    # We stop right after timestep k=20 (i.e. the last sensor observation)
    for k, obs_vector_z_k in enumerate(z_k, start=1):
        # Print the current timestep
        print(f'Timestep k={k}')

        # Run the Extended Kalman Filter and store the
        # near-optimal state and covariance estimates
        optimal_state_estimate_k, covariance_estimate_k = ekf(
            obs_vector_z_k,  # Most recent sensor measurement
            state_estimate_k_minus_1,  # Our most recent estimate of the state
            control_vector_k_minus_1,  # Our most recent control input
            P_k_minus_1,  # Our most recent state covariance matrix
            dk)  # Time interval

        # Get ready for the next timestep by updating the variable values
        state_estimate_k_minus_1 = optimal_state_estimate_k
        P_k_minus_1 = covariance_estimate_k

        # Print a blank line
        print()


# Program starts running here with the main method
main()

产出如下：

Timestep k=1
State Estimate Before EKF=[41.01  2.01  0.01  0.01  0.01]
[ 41.392088   2.165009  11.323501 190.255528   1.053575]
Observation=[ 41.392088   2.165009  11.323501 190.255528   1.053575]
State Estimate After EKF=[ 41.258058   2.106673   7.545667 126.833685   0.69905 ]

Timestep k=2
State Estimate Before EKF=[ 82.268058   4.116673   7.555667 126.843685   0.70905 ]
[ 41.391986   2.165057  12.493136 148.305246   1.169635]
Observation=[ 41.391986   2.165057  12.493136 148.305246   1.169635]
State Estimate After EKF=[ 56.714263   2.890663  10.635335 140.250911   0.990666]

Timestep k=3
State Estimate Before EKF=[ 97.724263   4.900663  10.645335 140.260911   1.000666]
[ 41.391909   2.165148  11.487028 139.273089  -1.006109]
Observation=[ 41.391909   2.165148  11.487028 139.273089  -1.006109]
State Estimate After EKF=[ 62.845663   3.201059  11.160193 139.643211  -0.247814]

Timestep k=4
State Estimate Before EKF=[103.855663   5.211059  11.170193 139.653211  -0.237814]
[ 41.391816   2.165256  13.726776 139.273089   2.239748]
Observation=[ 41.391816   2.165256  13.726776 139.273089   2.239748]
State Estimate After EKF=[ 65.235466   3.322017  12.744444 139.412044   1.287588]

Timestep k=5
State Estimate Before EKF=[106.245466   5.332017  12.754444 139.422044   1.297588]
[ 41.391733   2.165357  12.466767 137.555364  -1.260009]
Observation=[ 41.391733   2.165357  12.466767 137.555364  -1.260009]
State Estimate After EKF=[ 66.156076   3.368665  12.570463 138.262152  -0.289329]

Timestep k=6
State Estimate Before EKF=[107.166076   5.378665  12.580463 138.272152  -0.279329]
[ 41.391668   2.165438   9.901314 137.555364  -2.565454]
Observation=[ 41.391668   2.165438   9.901314 137.555364  -2.565454]
State Estimate After EKF=[ 66.508869   3.386591  10.918469 137.82297   -1.698419]

Timestep k=7
State Estimate Before EKF=[107.518869   5.396591  10.928469 137.83297   -1.688419]
[ 41.391628   2.165487   6.047908 137.582728  -3.853406]
Observation=[ 41.391628   2.165487   6.047908 137.582728  -3.853406]
State Estimate After EKF=[ 66.643776   3.393477   7.905934 137.672131  -3.032636]

Timestep k=8
State Estimate Before EKF=[107.653776   5.403477   7.915934 137.682131  -3.022636]
[ 41.391619   2.165498   1.332408 137.595421  -4.7155  ]
Observation=[ 41.391619   2.165498   1.332408 137.595421  -4.7155  ]
State Estimate After EKF=[ 66.695326   3.396116   3.84091  137.622361  -4.075064]

Timestep k=9
State Estimate Before EKF=[107.705326   5.406116   3.85091  137.632361  -4.065064]
[ 41.391619   2.165498   0.       137.595421  -1.332408]
Observation=[ 41.391619   2.165498   0.       137.595421  -1.332408]
State Estimate After EKF=[ 66.71502    3.397124   1.464736 137.603351  -2.38237 ]

Timestep k=10
State Estimate Before EKF=[107.72502    5.407124   1.474736 137.613351  -2.37237 ]
[ 41.391619   2.165498   0.       137.595421   0.      ]
Observation=[ 41.391619   2.165498   0.       137.595421   0.      ]
State Estimate After EKF=[ 66.722543   3.397509   0.557119 137.59609   -0.912345]

Timestep k=11
State Estimate Before EKF=[107.732543   5.407509   0.567119 137.60609   -0.902345]
[ 41.391619   2.165498  -1.930117 137.595421   1.023591]
Observation=[ 41.391619   2.165498  -1.930117 137.595421   1.023591]
State Estimate After EKF=[ 66.725417   3.397656  -0.982438 137.593316   0.281769]

Timestep k=12
State Estimate Before EKF=[107.735417   5.407656  -0.972438 137.603316   0.291769]
[ 41.391619   2.165498  -1.710974 137.595421   2.246764]
Observation=[ 41.391619   2.165498  -1.710974 137.595421   2.246764]
State Estimate After EKF=[ 66.726515   3.397712  -1.435059 137.592256   1.493842]

Timestep k=13
State Estimate Before EKF=[107.736515   5.407712  -1.425059 137.602256   1.503842]
[ 41.391619   2.165498  -0.375267 137.595421   3.334014]
Observation=[ 41.391619   2.165498  -0.375267 137.595421   3.334014]
State Estimate After EKF=[ 66.726934   3.397733  -0.782432 137.591851   2.62877 ]

Timestep k=14
State Estimate Before EKF=[107.736934   5.407733  -0.772432 137.601851   2.63877 ]
[ 41.391619   2.165498   2.287329 137.595421   4.192694]
Observation=[ 41.391619   2.165498   2.287329 137.595421   4.192694]
State Estimate After EKF=[ 66.727094   3.397741   1.112424 137.591697   3.592968]

Timestep k=15
State Estimate Before EKF=[107.737094   5.407741   1.122424 137.601697   3.602968]
[ 41.391619   2.165498   6.132224 137.595421   4.717386]
Observation=[ 41.391619   2.165498   6.132224 137.595421   4.717386]
State Estimate After EKF=[ 66.727155   3.397744   4.21247  137.591638   4.285536]

Timestep k=16
State Estimate Before EKF=[107.737155   5.407744   4.22247  137.601638   4.295536]
[ 41.391619   2.165498   7.416489 137.595421   4.833027]
Observation=[ 41.391619   2.165498   7.416489 137.595421   4.833027]
State Estimate After EKF=[ 66.727178   3.397746   6.190302 137.591615   4.621543]

Timestep k=17
State Estimate Before EKF=[107.737178   5.407746   6.200302 137.601615   4.631543]
[ 41.391619   2.165498   7.555815 137.595421   4.833027]
Observation=[ 41.391619   2.165498   7.555815 137.595421   4.833027]
State Estimate After EKF=[ 66.727187   3.397746   7.031875 137.591607   4.749887]

Timestep k=18
State Estimate Before EKF=[107.737187   5.407746   7.041875 137.601607   4.759887]
[ 41.391619   2.165498   9.685403 137.595421   4.833027]
Observation=[ 41.391619   2.165498   9.685403 137.595421   4.833027]
State Estimate After EKF=[ 66.727191   3.397746   8.669485 137.591603   4.79891 ]

Timestep k=19
State Estimate Before EKF=[107.737191   5.407746   8.679485 137.601603   4.80891 ]
[ 41.391619   2.165498   9.696774 137.595421   4.833027]
Observation=[ 41.391619   2.165498   9.696774 137.595421   4.833027]
State Estimate After EKF=[ 66.727192   3.397746   9.302024 137.591602   4.817635]

Timestep k=20
State Estimate Before EKF=[107.737192   5.407746   9.312024 137.601602   4.827635]
[ 41.391619   2.165498   8.40375  137.595421   4.833027]
Observation=[ 41.391619   2.165498   8.40375  137.595421   4.833027]
State Estimate After EKF=[ 66.727193   3.397746   8.744499 137.591602   4.824787]

Answer 1

实际上，你的卡尔曼是线性的，而不是扩展的卡尔曼滤波器。在定义变量ekf时，您的问题是函数state_estimate_k。公式应该是x(estimate) = F*x + B*u。只需将代码更改为：

state_estimate_k = A_k_minus_1 @ (
        state_estimate_k_minus_1) +  (
                           control_vector_k_minus_1) @ (
                           process_noise_v_k_minus_1)