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（正确）中，我得到：

图1

在python（不正确）中，我得到：

图2

出了什么问题？有没有更好的方法在 python 中做到这一点？

谢谢，斯科特

编辑：链接到数据文件 - 示例数据 1. csv 文件包含输入到过滤器公式中的加速度计、陀螺仪数据以及在 excel 中计算的那些值。2. excel 文件包含所有原始数据（上面没有提到的步骤，但我已经三重检查，并且相当于输入过滤器公式的点）。

编辑 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是采样频率，也许你应该尝试反转它？

编辑：好的，既然你发布了数据，我就玩弄这个。这是我得到正确结果的程序。

您的代码看起来基本正确，所以我认为您在收集值的代码中一定犯了一个错误。我不太确定，因为你的变量名让我感到困惑。

我使用 anamedtuple和作为名称，我使用了 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 回答