Question

我一直在摸不着头脑。

数据来自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 http://i48.tinypic.com/qpj63b.png

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

pic2 http://i47.tinypic.com/18m3j5.png

出了什么问题？在python中有更好的方法吗？

谢谢，斯科特

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

编辑2：更新 - 结果我的代码有效。这是松散的调试。 fs应为fs = 0.01。在我的代码中，我有fs = 1/100，在脚本中最终= 0。

Answer 1

你的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_line是CSV文件中保存的cfX值与新计算的cfx值之间的差异。正如你在图中看到的那样，这是0的直线，所以我的计算与你的计算很吻合。

所以我猜名字的映射是：

your_name       my_name
________________________
r1_angle_cfx    cfx
r1_alpha_x      rotX
r1_angle_ax     acc_angX

补充过滤器代码不起作用

1 个答案: