在原始加速度数据上使用 PyKalman 计算位置

Posted

技术标签:

【中文标题】在原始加速度数据上使用 PyKalman 计算位置【英文标题】:Using PyKalman on Raw Acceleration Data to Calculate Position 【发布时间】:2018-04-22 23:00:19 【问题描述】:

这是我在 *** 上的第一个问题,如果我措辞不当,我深表歉意。我正在编写代码以从 IMU 获取原始加速度数据,然后将其集成以更新对象的位置。目前,此代码每毫秒读取一个新的加速度计,并使用它来更新位置。我的系统有很多噪音,即使使用我实施的 ZUPT 方案,也会由于复合错误导致疯狂的读数。我知道卡尔曼滤波器理论上是这种场景的理想选择,我想使用 pykalman 模块而不是自己构建一个。

我的第一个问题是,pykalman 可以这样实时使用吗?从文档来看,在我看来,您必须记录所有测量结果,然后执行平滑操作,这不切实际,因为我想每毫秒递归过滤一次。

我的第二个问题是,对于转换矩阵,我只能将 pykalman 应用于加速度数据,还是可以以某种方式将双重积分包含到位置?该矩阵会是什么样子?

如果 pykalman 在这种情况下不实用,还有其他方法可以实现卡尔曼滤波器吗?提前谢谢!

【问题讨论】:

【参考方案1】:

在这种情况下,您可以使用卡尔曼滤波器,但您的位置估计很大程度上取决于您的加速度信号的精度。卡尔曼滤波器实际上对于融合多个信号很有用。因此,一个信号的误差可以由另一个信号来补偿。理想情况下,您需要使用基于不同物理效应的传感器(例如用于加速度的 IMU、用于位置的 GPS、用于速度的里程计)。

在这个答案中,我将使用来自两个加速度传感器(都在 X 方向)的读数。这些传感器之一是广泛而精确的。第二个便宜很多。因此,您将看到传感器精度对位置和速度估计的影响。

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

让我们从建模开始。假设您只有 X 方向的加速度读数。所以你的观察看起来像

现在您需要定义最小的数据集,它可以完整地描述您的系统在每个时间点。这将是系统状态。

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

现在您需要描述系统动力学。根据此信息,过滤器将根据前一个状态预测新状态。

在我的情况下 dt=0.01s。使用这个矩阵,滤波器将整合加速度信号来估计速度和位置。

观察协方差 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您能否请提供数据集和load_data()的实现?从一天开始,我一直在为此苦苦挣扎,我尝试过的所有数据集要么不提供基本事实,要么需要重新格式化。非常感谢! 你好,米海,给我一些时间来查找我使用的数据。 嗨安东,你运气好吗? 你好米海,很遗憾我找不到原始数据。您可以使用我添加到答案中的简单 matlab 脚本来为给定的传感器方差生成传感器数据。我希望您可以将它用于您的研究。 @Anton 你能告诉我们昂贵和便宜的加速度计名称吗?我想查一下他们的数据表。

以上是关于在原始加速度数据上使用 PyKalman 计算位置的主要内容,如果未能解决你的问题,请参考以下文章

使用原始加速度计和磁力计数据计算磁航向

如何在 Python 中使用卡尔曼滤波器获取位置数据?

智能手机GPS定位算法研究

pykalman 标准 filtercorrect 模块中的“观察偏移”和“预测状态均值”是啥意思?

使用加速度计+陀螺仪的小汽车模型位置计算

原始战争之主界面滑动效果