我正在尝试使用 Python 编写卡尔曼滤波器，最终用 C++ 编写一个卡尔曼滤波器，并在 Arduino 中使用它来进行气压和惯性高度估计（一维位置）。
现在，我正在使用线性函数（速度 = 1）“模拟”真实位置和测量位置，同时在模拟位置测量中插入人工噪声。
虽然卡尔曼仅使用测量位置返回明显良好的结果，但它无法纠正错误的加速度测量值（例如恒定的 1.0 测量加速度输入）。
位置向加速度方向倾斜，并且滤波后的加速度不会通过位置测量得到校正。
这是卡尔曼滤波器的预期行为，还是我应该如何修改我的代码和/或滤波器以使卡尔曼对这些行为正确？
主要.py：
import numpy as np
import math
import matplotlib.pyplot as plt
from kalman import *
timestep = 0.01
time = 0.0
meas_pos = 0.0
kalman = Kalman()
list_time = []
list_real_pos = []
list_meas_pos = []
list_k_pos = []
list_k_vel = []
list_k_acc = []
while(time < 10.0):
list_time.append(time) # LIST
real_pos = 1.0 * time # Position 'function'
list_real_pos.append(real_pos) # LIST
kalman.kalman_predict(timestep) # KALMAN PREDICTION STEP
# Noise Coefficients
noise_coeff = 1 / 1000
noise_coeff_acc = 1 / 10000
meas_pos = real_pos + float(np.random.randint(-1000, 1000)) * noise_coeff # Calculate Measured Position
# Accelerometer Value is artificial, to simulate a broken sensor for example
if time > 3:
kalman.kalman_update_pos(meas_pos) # Then just update using position sensor
elif time > 1:
kalman.kalman_update(meas_pos, 10.0) # Then use both position sensor and accelerometer
else:
kalman.kalman_update_acc(10.0) # First update using only accelerometer
list_k_pos.append(kalman.k_pos) # LIST
list_meas_pos.append(meas_pos) # LIST
time += timestep # Advance in time
# PLOTTING
plt.figure()
plt.plot(list_time, list_real_pos, color = 'red', linestyle = 'dashed', label = 'Position')
plt.plot(list_time, list_meas_pos, color = 'purple', linestyle = 'solid', label = 'Measured Position')
plt.plot(list_time, list_k_pos, color = 'steelblue', linestyle = 'solid', label = 'Kalman Position')
plt.legend(loc = 'upper right')
plt.grid()
plt.show()
卡尔曼.py：
import numpy as np
from numpy.linalg import inv
import math
class Kalman:
def __init__(self):
self.k_pos = 0.0
self.k_vel = 0.0
self.k_acc = 0.0
self. x_hat = np.zeros(3)
self. Zk = np.zeros(3)
self. Pk = np.zeros((3, 3))
self. Rk = np.zeros((3, 3))
self. Qk = np.zeros((3, 3))
self. Fk = np.zeros((3, 3))
self. K = np.zeros((3, 3))
self. Hk = np.zeros((3, 3))
def kalman_predict(self, dt):
self.Fk = np.array([[1.0, dt, 0.5 * dt * dt],
[0.0, 1.0, dt],
[0.0, 0.0, 1.0]])
self.Qk = np.array([[1.0, 0.0, 0.0],
[0.0, 1.0, 0.0],
[0.0, 0.0, 1.0]])
self.Qk *= dt
self.x_hat = self.Fk @ self.x_hat
self.Pk = (self.Fk @ self.Pk @ self.Fk.T) + self.Qk
self.k_pos = self.x_hat[0]
self.k_vel = self.x_hat[1]
self.k_acc = self.x_hat[2]
return
def kalman_update(self, meas_pos, meas_acc):
self.Hk = np.array([[1.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 1.0]])
self.Rk = np.array([[1.0, 0.0, 0.0],
[0.0, 1.0, 0.0],
[0.0, 0.0, 1.0]])
self.Zk = np.array([meas_pos, 0, meas_acc])
self.K = self.Pk @ self.Hk.T @ inv((self.Hk @ self.Pk @ self.Hk.T) + self.Rk)
self.x_hat = self.x_hat + self.K @ (self.Zk - (self.Hk @ self.x_hat))
self.Pk = self.Pk - self.K @ self.Hk @ self.Pk
self.k_pos = self.x_hat[0]
self.k_vel = self.x_hat[1]
self.k_acc = self.x_hat[2]
return
def kalman_update_acc(self, meas_acc):
self.Hk = np.array([[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 1.0]])
self.Rk = np.array([[1.0, 0.0, 0.0],
[0.0, 1.0, 0.0],
[0.0, 0.0, 1.0]])
self.Zk = np.array([0, 0, meas_acc])
self.K = self.Pk @ self.Hk.T @ inv((self.Hk @ self.Pk @ self.Hk.T) + self.Rk)
self.x_hat = self.x_hat + self.K @ (self.Zk - (self.Hk @ self.x_hat))
self.Pk = self.Pk - self.K @ self.Hk @ self.Pk
self.k_pos = self.x_hat[0]
self.k_vel = self.x_hat[1]
self.k_acc = self.x_hat[2]
print(self.x_hat)
return
def kalman_update_pos(self, meas_pos):
self.Hk = np.array([[1.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0]])
self.Rk = np.array([[1.0, 0.0, 0.0],
[0.0, 1.0, 0.0],
[0.0, 0.0, 1.0]])
self.Zk = np.array([meas_pos, 0, 0])
self.K = self.Pk @ self.Hk.T @ inv((self.Hk @ self.Pk @ self.Hk.T) + self.Rk)
self.x_hat = self.x_hat + self.K @ (self.Zk - (self.Hk @ self.x_hat))
self.Pk = self.Pk - self.K @ self.Hk @ self.Pk
self.k_pos = self.x_hat[0]
self.k_vel = self.x_hat[1]
self.k_acc = self.x_hat[2]
return