在原始加速度数据上使用 PyKalman 计算位置
Using PyKalman on Raw Acceleration Data to Calculate Position
这是我在 Whosebug 上的第一个问题，所以如果我措辞不当，我深表歉意。我正在编写代码以从 IMU 获取原始加速度数据，然后对其进行整合以更新对象的位置。目前这段代码每毫秒获取一个新的加速度计读数，并使用它来更新位置。我的系统有很多噪音，即使使用我实施的 ZUPT 方案，也会由于复合误差导致疯狂读数。我知道卡尔曼滤波器在理论上是这种情况的理想选择，我想使用 pykalman 模块而不是自己构建一个。
我的第一个问题是，pykalman可以这样实时使用吗？从文档来看，在我看来，您必须记录所有测量值，然后执行平滑操作，这不切实际，因为我想每毫秒递归过滤一次。
我的第二个问题是，对于转换矩阵，我可以只将 pykalman 应用于加速度数据本身，还是可以以某种方式将二重积分包含在位置中？该矩阵会是什么样子？
如果 pykalman 在这种情况下不实用，有没有其他方法可以实现卡尔曼滤波器？提前致谢！
在这种情况下您可以使用卡尔曼滤波器，但您的位置估计在很大程度上取决于加速度信号的精度。卡尔曼滤波器实际上可用于融合多个信号。因此一个信号的误差可以由另一个信号补偿。理想情况下，您需要使用基于不同物理效应的传感器（例如，IMU 用于加速度，GPS 用于位置，里程计用于速度）。
在这个答案中，我将使用来自两个加速度传感器（均在 X 方向）的读数。这些传感器之一是广泛而精确的。第二个便宜很多。所以你会看到传感器精度对位置和速度估计的影响。
您已经提到了 ZUPT 方案。我只想添加一些注意事项：对俯仰角进行良好的估计非常重要，以消除 X 加速度中的重力分量。如果您使用 Y 和 Z 加速度，则需要俯仰角和横滚角。
让我们从建模开始。假设您只有 X 方向的加速度读数。所以你的观察看起来像
现在您需要定义最小的数据集，完整地描述您的系统在每个时间点的情况。它将是系统状态。
测量域和状态域之间的映射由观察矩阵定义：
现在您需要描述系统动力学。根据此信息，过滤器将根据前一个状态预测新状态。
在我的例子中，dt=0.01s。使用此矩阵，过滤器将对加速度信号进行积分以估计速度和位置。
观测协方差 R 可以用传感器读数的方差来描述。在我的例子中，我的观察中只有一个信号，因此观察协方差等于 X 加速度的方差（该值可以根据您的传感器数据表计算）。
您通过转移协方差 Q 来描述系统噪声。矩阵值越小，系统噪声越小。 Filter 会变硬，估计会延迟。与新测量相比，系统过去的权重会更高。否则，过滤器将更加灵活，并且会对每个新测量做出强烈反应。
现在一切准备就绪，可以配置 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');
模拟数据看起来与上面的数据非常相似。
这是我在 Whosebug 上的第一个问题，所以如果我措辞不当，我深表歉意。我正在编写代码以从 IMU 获取原始加速度数据，然后对其进行整合以更新对象的位置。目前这段代码每毫秒获取一个新的加速度计读数，并使用它来更新位置。我的系统有很多噪音，即使使用我实施的 ZUPT 方案，也会由于复合误差导致疯狂读数。我知道卡尔曼滤波器在理论上是这种情况的理想选择，我想使用 pykalman 模块而不是自己构建一个。
我的第一个问题是，pykalman可以这样实时使用吗？从文档来看，在我看来，您必须记录所有测量值，然后执行平滑操作，这不切实际，因为我想每毫秒递归过滤一次。
我的第二个问题是，对于转换矩阵，我可以只将 pykalman 应用于加速度数据本身，还是可以以某种方式将二重积分包含在位置中？该矩阵会是什么样子？
如果 pykalman 在这种情况下不实用，有没有其他方法可以实现卡尔曼滤波器？提前致谢！
在这种情况下您可以使用卡尔曼滤波器，但您的位置估计在很大程度上取决于加速度信号的精度。卡尔曼滤波器实际上可用于融合多个信号。因此一个信号的误差可以由另一个信号补偿。理想情况下，您需要使用基于不同物理效应的传感器（例如，IMU 用于加速度，GPS 用于位置，里程计用于速度）。
在这个答案中，我将使用来自两个加速度传感器（均在 X 方向）的读数。这些传感器之一是广泛而精确的。第二个便宜很多。所以你会看到传感器精度对位置和速度估计的影响。
您已经提到了 ZUPT 方案。我只想添加一些注意事项：对俯仰角进行良好的估计非常重要，以消除 X 加速度中的重力分量。如果您使用 Y 和 Z 加速度，则需要俯仰角和横滚角。
让我们从建模开始。假设您只有 X 方向的加速度读数。所以你的观察看起来像
现在您需要定义最小的数据集，完整地描述您的系统在每个时间点的情况。它将是系统状态。
测量域和状态域之间的映射由观察矩阵定义：
现在您需要描述系统动力学。根据此信息，过滤器将根据前一个状态预测新状态。
在我的例子中，dt=0.01s。使用此矩阵，过滤器将对加速度信号进行积分以估计速度和位置。
观测协方差 R 可以用传感器读数的方差来描述。在我的例子中，我的观察中只有一个信号，因此观察协方差等于 X 加速度的方差（该值可以根据您的传感器数据表计算）。
您通过转移协方差 Q 来描述系统噪声。矩阵值越小，系统噪声越小。 Filter 会变硬，估计会延迟。与新测量相比，系统过去的权重会更高。否则，过滤器将更加灵活，并且会对每个新测量做出强烈反应。
现在一切准备就绪，可以配置 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');
模拟数据看起来与上面的数据非常相似。