Проблема с фильтром Калмана: гироскоп измеряет дельта-вращение, но угол AoA — это абсолютный угол ⇐ Python

[Anonymous]

Я работаю над проектом, в котором объединяю данные гироскопа (который измеряет дельта-вращение) с измерениями AoA (угол прибытия) (абсолютные углы в градусах) с помощью фильтра Калмана. Цель состоит в том, чтобы оценить абсолютную ориентацию объекта.
Однако результаты фильтра, похоже, расходятся или ведут себя неожиданно, и я подозреваю, что проблема заключается в фундаментальном различии между ними. типы измерений:
Гироскоп: измеряет скорость вращения (изменение дельты с течением времени).
AoA: обеспечивает измерение абсолютного угла (например, 20 градусов).
Вот реализация фильтра Калмана, я используя:
def predict(posterior, angular_velocity, delta_t):
x, P = posterior # mean and variance of posterior
dx, Q = angular_velocity # mean and variance of angular velocity
x = x + (dx * delta_t)
P = P + Q * delta_t**2
return (x, P)

def update(prior, measurement):
x, P = prior # mean and variance of prior
z, R = measurement # mean and variance of measurement

y = z - x # residual
K = P / (P + R) # Kalman gain
x = x + K * y # posterior mean
P = (1 - K) * P # posterior variance
return (x, P)

# Standard deviations (adjust if needed)
gyroscope_std = 0.013 # Estimated 0.013 dps
aoa_std = 10.27 # Estimated 10.27 deg

# Initial state from the first measurement
start = 0
initial_angle = df.loc[start, 'theta_deg']
x = (initial_angle, aoa_std**2) # (mean, variance)

predictions = []
timestamp = df.loc[0, 'timestamp']

for i, row in df[start:].iterrows():
if i == 0:
# Initial prediction is just the initial state
predictions.append(x[0])
continue

delta_t = row['timestamp'] - timestamp
timestamp = row['timestamp']
angular_velocity = row['gyroscope_dps']
aoa = row['theta_deg']

# Use the gyroscope reading as the process model input
gyro_reading = (angular_velocity, gyroscope_std**2)
prior = predict(x, gyro_reading, delta_t)

# Update with the AoA measurement
x = update(prior, (aoa, aoa_std**2))

predictions.append(x[0])



Подробнее здесь: https://stackoverflow.com/questions/792 ... lute-angle