使用PyKalman处理原始加速度数据以计算位置

Question

这是我在Stackoverflow上的第一个问题，如果我的措辞不当，请见谅。我正在编写代码，以获取IMU的原始加速度数据，然后集成它来更新对象的位置。目前，此代码每毫秒采取一次新的加速度计读数，并使用该读数来更新位置。我的系统有很多噪声，导致由于复合误差而产生疯狂的读数，即使我实施了ZUPT方案。我知道Kalman滤波器在理论上非常适合这种情况，我想使用pykalman模块而不是自己构建一个。

我的第一个问题是，pykalman能否像这样实时使用？从文档中看来，您必须记录所有测量值，然后执行平滑操作，这对我来说不实际，因为我想每毫秒递归地过滤。

我的第二个问题是，对于转移矩阵，我只能将pykalman应用于加速度数据本身，还是可以某种方式包括双重积分到位置？那个矩阵会是什么样子？

如果pykalman不适用于此情况，是否有其他方法可以实现Kalman滤波器？提前谢谢！

- Alex

Answer 1

在这种情况下，您可以使用卡尔曼滤波器，但是您的位置估计将严重依赖于加速度信号的精度。卡尔曼滤波器实际上用于多个信号的融合。因此，一个信号的误差可以通过另一个信号来补偿。理想情况下，您需要使用基于不同物理效应的传感器（例如IMU用于加速度，GPS用于位置，里程表用于速度）。

在本答案中，我将使用两个加速度传感器的读数（均为X方向）。其中一个传感器价格昂贵且精确。第二个传感器要便宜得多。因此，您将看到传感器精度对位置和速度估计的影响。

您已经提到了ZUPT方案。我只想添加一些注释：非常重要的是要很好地估计俯仰角，以消除X加速度中的重力分量。如果您使用Y和Z加速度，则需要俯仰和滚动角度。

让我们从建模开始。假设您只有X方向的加速度读数。因此，您的观测将如下所示：

现在，你需要定义最小的数据集，以完全描述系统在每个时间点上的状态。这将是系统状态。

测量和状态之间的映射由观测矩阵定义：

现在你需要描述系统动态。根据这些信息，过滤器将基于先前的状态预测一个新状态。

在我的情况下，dt=0.01秒。使用这个矩阵，滤波器将对加速度信号进行积分以估计速度和位置。

观测协方差R可以用传感器读数的方差来描述。在我的情况下，观测中只有一个信号，因此观测协方差等于X加速度的方差（该值可以根据传感器数据表计算）。

通过过渡协方差Q，您描述了系统噪声。矩阵值越小，系统噪声越小。滤波器会变得更加严格，估计会延迟。与新测量相比，系统过去的权重将更高。否则，滤波器将更加灵活，并对每个新测量做出强烈反应。

现在一切准备就绪，可以配置Pykalman。为了实时使用它，您必须使用filter_update函数。

from pykalman import KalmanFilter
import numpy as np
import matplotlib.pyplot as plt

load_data()

# Data description
#  Time
#  AccX_HP - high precision acceleration signal
#  AccX_LP - low precision acceleration signal
#  RefPosX - real position (ground truth)
#  RefVelX - real velocity (ground truth)

# switch between two acceleration signals
use_HP_signal = 1

if use_HP_signal:
    AccX_Value = AccX_HP
    AccX_Variance = 0.0007
else:    
    AccX_Value = AccX_LP
    AccX_Variance = 0.0020


# time step
dt = 0.01

# transition_matrix  
F = [[1, dt, 0.5*dt**2], 
     [0,  1,       dt],
     [0,  0,        1]]

# observation_matrix   
H = [0, 0, 1]

# transition_covariance 
Q = [[0.2,    0,      0], 
     [  0,  0.1,      0],
     [  0,    0,  10e-4]]

# observation_covariance 
R = AccX_Variance

# initial_state_mean
X0 = [0,
      0,
      AccX_Value[0, 0]]

# initial_state_covariance
P0 = [[  0,    0,               0], 
      [  0,    0,               0],
      [  0,    0,   AccX_Variance]]

n_timesteps = AccX_Value.shape[0]
n_dim_state = 3
filtered_state_means = np.zeros((n_timesteps, n_dim_state))
filtered_state_covariances = np.zeros((n_timesteps, n_dim_state, n_dim_state))

kf = KalmanFilter(transition_matrices = F, 
                  observation_matrices = H, 
                  transition_covariance = Q, 
                  observation_covariance = R, 
                  initial_state_mean = X0, 
                  initial_state_covariance = P0)

# iterative estimation for each new measurement
for t in range(n_timesteps):
    if t == 0:
        filtered_state_means[t] = X0
        filtered_state_covariances[t] = P0
    else:
        filtered_state_means[t], filtered_state_covariances[t] = (
        kf.filter_update(
            filtered_state_means[t-1],
            filtered_state_covariances[t-1],
            AccX_Value[t, 0]
        )
    )


f, axarr = plt.subplots(3, sharex=True)

axarr[0].plot(Time, AccX_Value, label="Input AccX")
axarr[0].plot(Time, filtered_state_means[:, 2], "r-", label="Estimated AccX")
axarr[0].set_title('Acceleration X')
axarr[0].grid()
axarr[0].legend()
axarr[0].set_ylim([-4, 4])

axarr[1].plot(Time, RefVelX, label="Reference VelX")
axarr[1].plot(Time, filtered_state_means[:, 1], "r-", label="Estimated VelX")
axarr[1].set_title('Velocity X')
axarr[1].grid()
axarr[1].legend()
axarr[1].set_ylim([-1, 20])

axarr[2].plot(Time, RefPosX, label="Reference PosX")
axarr[2].plot(Time, filtered_state_means[:, 0], "r-", label="Estimated PosX")
axarr[2].set_title('Position X')
axarr[2].grid()
axarr[2].legend()
axarr[2].set_ylim([-10, 1000])

plt.show()

使用更好的IMU传感器时，估计的位置与实际位置完全相同：

价格更便宜的传感器结果明显较差：

我希望能够帮助你。如果你有任何问题，我会尽力回答。 更新 如果你想尝试不同的数据，你可以很容易地生成它们（不幸的是，我没有原始数据了）。

这里有一个简单的 Matlab 脚本，可以生成参考、良好和差劣的传感器组。

clear;

dt = 0.01;
t=0:dt:70;

accX_var_best = 0.0005; % (m/s^2)^2
accX_var_good = 0.0007; % (m/s^2)^2
accX_var_worst = 0.001; % (m/s^2)^2

accX_ref_noise = randn(size(t))*sqrt(accX_var_best);
accX_good_noise = randn(size(t))*sqrt(accX_var_good);
accX_worst_noise = randn(size(t))*sqrt(accX_var_worst);

accX_basesignal = sin(0.3*t) + 0.5*sin(0.04*t);

accX_ref = accX_basesignal + accX_ref_noise;
velX_ref = cumsum(accX_ref)*dt;
distX_ref = cumsum(velX_ref)*dt;


accX_good_offset = 0.001 + 0.0004*sin(0.05*t);

accX_good = accX_basesignal + accX_good_noise + accX_good_offset;
velX_good = cumsum(accX_good)*dt;
distX_good = cumsum(velX_good)*dt;


accX_worst_offset = -0.08 + 0.004*sin(0.07*t);

accX_worst = accX_basesignal + accX_worst_noise + accX_worst_offset;
velX_worst = cumsum(accX_worst)*dt;
distX_worst = cumsum(velX_worst)*dt;

subplot(3,1,1);
plot(t, accX_ref);
hold on;
plot(t, accX_good);
plot(t, accX_worst);
hold off;
grid minor;
legend('ref', 'good', 'worst');
title('AccX');

subplot(3,1,2);
plot(t, velX_ref);
hold on;
plot(t, velX_good);
plot(t, velX_worst);
hold off;
grid minor;
legend('ref', 'good', 'worst');
title('VelX');

subplot(3,1,3);
plot(t, distX_ref);
hold on;
plot(t, distX_good);
plot(t, distX_worst);
hold off;
grid minor;
legend('ref', 'good', 'worst');
title('DistX');

模拟数据看起来与上面的数据几乎相同。

- Anton

---

这太棒了。@Anton，你能否也提供load_data()的数据集和实现？我已经为此苦苦挣扎了一天，而我尝试过的所有数据集都没有提供基准真相或需要重新格式化。非常感谢！ - Mihai Galos

你好Mihai，给我一些时间去找我使用过的数据。 - Anton

嗨，安东，你有什么进展了吗？ - Mihai Galos

你好Mihai，很抱歉我找不到原始数据。你可以使用我在答案中添加的简单Matlab脚本来生成给定传感器方差的传感器数据。希望你能用它来进行研究。 - Anton

@Oscar，是的，我有一些使用imufilter函数的经验。它有一个很好的内部数学描述，可以为卡尔曼滤波器提供所需的所有矩阵。因此，如果您对imufilter的输出感到满意，您可以重新实现它以适用于pykalman库。我之前尝试在matlab中实现过这个功能。 - Anton