2

私はあまりにも長い間頭を悩ませてきました。

データは、3D 加速度計と 3D ジャイロから取得されます。ドリフトを制御するために補完的なフィルターを使用しています。

私はそれをExcelで動作させていますが、このpythonコードで同じことをするようには見えません:

r1_angle_cfx = np.zeros(len(r1_angle_ax))
r1_angle_cfx[0] = r1_angle_ax[0]

for i in xrange(len(r1_angle_ax)-1):
        j = i + 1
        r1_angle_cfx[j] = 0.98 *(r1_angle_cfx[i] + r1_alpha_x[j]*fs) + (0.02 * r1_angle_ax[j])    #complementary filter

Excel（正しい）では、次のようになります。

pic1

python (正しくない) では、次のようになります。

pic2

何がうまくいかないのですか？Pythonでこれを行うより良い方法はありますか?

ありがとう、スコット

編集: データ ファイルへのリンク - サンプル データ 1. csv ファイルには、フィルター式に入力された加速度計、ジャイロ データ、および Excel で計算された値が含まれています。2. Excel ファイルにはすべての生データが含まれています (手順は上記に記載されていませんが、3 回チェックしており、フィルター式に入力されるまでは同等です)。

編集 2: 更新 - 私のコードが機能することがわかりました。ずさんなデバッグでした。fs は fs = 0.01 である必要があります。私のコードでは fs = 1/100 で、スクリプトでは = 0 になります。

4

1 に答える 1

2

あなたの Python コードはかなり合理的に見えます。例のデータがなければ、それ以上のことはできません。

しかし、私は推測することができます。「補完フィルター」を調べたところ、それらを説明するリンクが見つかりました。

https://sites.google.com/site/myimuestimationexperience/filters/complementary-filter

このリンクは、あなたのものと非常によく似た方程式の例を示しています。

angle = (1-alpha)*(angle + gyro * dt) + (alpha)*(acc)

fsこれはでdtあり、dtとして計算され1/sampling_frequencyます。サンプリング周波数の場合fsは、反転してみてはいかがでしょうか。

編集：さて、データを投稿したので、これで遊んでみました。これが正しい結果を得る私のプログラムです。

あなたのコードは基本的に正しいように見えるので、値を収集したコードで間違いを犯したに違いないと思います。あなたの変数名が私を混乱させるので、よくわかりません。

名前には と を使用namedtupleし、CSV ファイルの列ヘッダーを使用しました (有効な Python 識別子を作成するためにスペースとピリオドを削除しました)。

import collections as coll
import csv
import matplotlib.pyplot as plt
import numpy as np
import sys


fs = 100.0
dt = 1.0/fs
alpha = 0.02

Sample = coll.namedtuple("Sample",
    "accZ accY accX rotZ rotY rotX r acc_angZ acc_angY acc_angX cfZ cfY cfX")

def samples_from_file(fname):
    with open(fname) as f:
        next(f)  # discard header row
        csv_reader = csv.reader(f, dialect='excel')

        for i, row in enumerate(csv_reader, 1):
            try:
                values = [float(x) for x in row]
                yield Sample(*values)
            except Exception:
                lst = list(row)
                print("Bad line %d: len %d '%s'" % (i, len(lst), str(lst)))


samples = list(samples_from_file("data.csv"))

cfx = np.zeros(len(samples))

# Excel formula: =R12
cfx[0] = samples[0].acc_angX
# Excel formula: =0.98*(U12+N13*0.01)+0.02*R13
# Excel: U is cfX  N is rotX  R is acc_angX
for i, s in enumerate(samples[1:], 1):
    cfx[i] = (1.0 - alpha) * (cfx[i-1] + s.rotX*dt) + (alpha * s.acc_angX)

check_line = [s.cfX - cf for s, cf in zip(samples, cfx)]

plt.figure(1)
plt.plot(check_line)
plt.plot(cfx)
plt.show()

check_linecfXは、CSV ファイルから保存された値と、新しく計算された値との差cfxです。プロットでわかるように、これは 0 での直線なので、私の計算はあなたの計算とよく一致しています。

したがって、名前のマッピングは次のようになります。

your_name       my_name
________________________
r1_angle_cfx    cfx
r1_alpha_x      rotX
r1_angle_ax     acc_angX
于 2013-02-28T00:43:45.337 に答える