我正在尝试实现一个卡尔曼滤波器，用于过滤速度、航向、加速度和偏航率的计算值。我还使用卡尔曼滤波器来预测未来值，并使用这些值来计算对象的未来地理位置。
过滤器似乎将预测值重置为 0。目前，我的下一个状态更新始终保持不变（nv = v、na = a 等..），但如果我更改为
nv = v + a * dt主要代码：
# 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
...
（一些警告）我对你想要做什么的信息有限。您发布的代码也无法重现您的确切问题，因此您的代码中可能还存在我不知道的其他情况。
当您有噪声动态和噪声测量，并试图获得当前状态的最佳估计时，卡尔曼滤波器非常有用。在您的过滤器中，您将估计状态（与真实状态不同）初始化为全零，然后传播它（这使您的情况下的所有状态保持不变）。我对传播状态保持为零并不感到惊讶；这是预期的行为。您缺少的最重要的事情是对
ukf.update为什么您所在州没有纬度和经度？