web-dev-qa-db-ja.com

生の加速度データでPyKalmanを使用して位置を計算する

これはStackoverflowに関する私の最初の質問ですので、言葉をおかしくなりましたら申し訳ありません。 IMUから生の加速度データを取得し、それを統合してオブジェクトの位置を更新するコードを書いています。現在、このコードはミリ秒ごとに新しい加速度計を読み取り、それを使用して位置を更新します。私のシステムには多くのノイズがあり、私が実装したZUPTスキームを使用しても、コンパウンディングエラーが原因で読み取りがおかしくなります。カルマンフィルターは理論的にはこのシナリオに理想的であることがわかっています。自分で作成するのではなく、pykalmanモジュールを使用したいと思います。

私の最初の質問は、pykalmanをこのようにリアルタイムで使用できるかどうかです。ドキュメンテーションから、すべての測定値の記録を取得してからスムーズな操作を実行する必要があるように見えます。ミリ秒ごとに再帰的にフィルタリングする必要があるため、これは実用的ではありません。

2つ目の質問は、遷移行列の場合、pykalmanのみを加速度データに適用できるのか、それとも二重積分を位置に含めることができるのか、です。そのマトリックスはどのように見えるでしょうか？

Pykalmanがこの状況で実用的でない場合、カルマンフィルターを実装する別の方法はありますか？前もって感謝します！

pythonnoisekalman-filterpykalman
13
Alex

この場合はカルマンフィルターを使用できますが、位置推定は加速度信号の精度に大きく依存します。カルマンフィルターは、いくつかの信号の融合に実際に役立ちます。したがって、1つの信号のエラーは別の信号で補正できます。理想的には、さまざまな物理的効果に基づくセンサーを使用する必要があります（たとえば、加速度にはIMU、位置にはGPS、速度にはオドメトリ）。

この回答では、2つの加速度センサー（両方ともX方向）からの読み取り値を使用します。これらのセンサーの1つは、拡張可能で正確です。 2つ目ははるかに安価です。したがって、位置と速度の推定に対するセンサーの精度の影響がわかります。

ZUPTスキームについてはすでに説明しました。 X加速度の重力成分を取り除くには、ピッチ角を適切に推定することが非常に重要です。 Y加速度とZ加速度を使用する場合は、ピッチ角とロール角の両方が必要です。

モデリングから始めましょう。 X方向の加速度の読み取り値だけがあると仮定します。だからあなたの観察は次のようになります

formula

次に、各時点でのシステムを完全に記述する最小のデータセットを定義する必要があります。システム状態になります。

formula

測定ドメインと状態ドメインの間のマッピングは、観測行列によって定義されます。

formula

formula

次に、システムのダイナミクスを説明する必要があります。この情報によると、フィルターは前の状態に基づいて新しい状態を予測します。

formula

私の場合、dt = 0.01sです。このマトリックスを使用して、フィルターは加速度信号を積分し、速度と位置を推定します。

観測共分散Rは、センサーの読み取り値の分散によって説明できます。私の場合、観測に信号が1つしかないため、観測の共分散は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センサーを使用すると、推定位置はグラウンドトゥルースとまったく同じになります。

position estimation based on a good IMU-sensor

安価なセンサーは著しく悪い結果をもたらします：

position estimation based on a cheap IMU-sensor

お役に立てれば幸いです。質問がある場合は、回答させていただきます。

[〜＃〜]更新[〜＃〜]

さまざまなデータを試してみたい場合は、簡単に生成できます（残念ながら、元のデータはもうありません）。

これは、リファレンス、良質および不​​良センサーセットを生成するための簡単な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');

シミュレートされたデータは、上記のデータとほとんど同じです。

simulated data for different sensor variances

11
Anton