给定位置测量,如何估计速度和加速度

信息处理 卡尔曼滤波器 估计 过滤
2022-01-05 09:53:35

我认为这很简单,但我天真的方法导致了一个非常嘈杂的结果。我在一个名为 t_angle.txt 的文件中有这个采样时间和位置:

0.768 -166.099892
0.837 -165.994148
0.898 -165.670052
0.958 -165.138245
1.025 -164.381218
1.084 -163.405838
1.144 -162.232704
1.213 -160.824051
1.268 -159.224854
1.337 -157.383270
1.398 -155.357666
1.458 -153.082809
1.524 -150.589943
1.584 -147.923012
1.644 -144.996872
1.713 -141.904221
1.768 -138.544807
1.837 -135.025749
1.896 -131.233063
1.957 -127.222366
2.024 -123.062325
2.084 -118.618355
2.144 -114.031906
2.212 -109.155006
2.271 -104.059753
2.332 -98.832321
2.399 -93.303795
2.459 -87.649956
2.520 -81.688499
2.588 -75.608597
2.643 -69.308281
2.706 -63.008308
2.774 -56.808586
2.833 -50.508270
2.894 -44.308548
2.962 -38.008575
3.021 -31.808510
3.082 -25.508537
3.151 -19.208565
3.210 -13.008499
3.269 -6.708527
3.337 -0.508461
3.397 5.791168
3.457 12.091141
3.525 18.291206
3.584 24.591179
3.645 30.791245
3.713 37.091217
3.768 43.291283
3.836 49.591255
3.896 55.891228
3.957 62.091293
4.026 68.391266
4.085 74.591331
4.146 80.891304
4.213 87.082100
4.268 92.961502
4.337 98.719368
4.397 104.172363
4.458 109.496956
4.518 114.523888
4.586 119.415550
4.647 124.088860
4.707 128.474464
4.775 132.714500
4.834 136.674385
4.894 140.481148
4.962 144.014626
5.017 147.388458
5.086 150.543938
5.146 153.436089
5.207 156.158638
5.276 158.624725
5.335 160.914001
5.394 162.984924
5.463 164.809685
5.519 166.447678

并且想要估计速度和加速度。我知道加速度是恒定的,在这种情况下约为 55 度/秒^2,直到速度约为 100 度/秒,然后加速度为零且速度恒定。最后加速度为 -55 度/秒^2。这是 scilab 代码,它给出了非常嘈杂且无法使用的估计,尤其是加速度。

clf()
clear
M=fscanfMat('t_angle.txt');
t=M(:,1);
len=length(t);
x=M(:,2);
dt=diff(t);
dx=diff(x);
v=dx./dt;
dv=diff(v);
a=dv./dt(1:len-2);
subplot(311), title("position"),
plot(t,x,'b');
subplot(312), title("velocity"),
plot(t(1:len-1),v,'g');
subplot(313), title("acceleration"),
plot(t(1:len-2),a,'r');

我正在考虑使用卡尔曼滤波器来获得更好的估计。这里合适吗?不知道如何制定滤波器方程,对卡尔曼滤波器不是很有经验。我认为状态向量是速度和加速度,而输入信号是位置。或者有没有比 KF 更简单的方法,可以提供有用的结果。

欢迎所有建议! 在此处输入图像描述

2个回答

一种方法是将问题转换为最小二乘平滑。这个想法是用移动窗口局部拟合多项式,然后评估多项式的​​导数。这个关于 Savitzky-Golay 过滤的答案有一些关于它如何用于非均匀采样的理论背景。

在这种情况下,代码可能更能说明该技术的好处/限制。以下numpy脚本将根据两个参数计算给定位置信号的速度和加速度:1)平滑窗口的大小,以及 2)局部多项式逼近的阶数。

# Example Usage:
# python sg.py position.dat 7 2

import math
import sys

import numpy as np
import numpy.linalg
import pylab as py

def sg_filter(x, m, k=0):
    """
    x = Vector of sample times
    m = Order of the smoothing polynomial
    k = Which derivative
    """
    mid = len(x) / 2        
    a = x - x[mid]
    expa = lambda x: map(lambda i: i**x, a)    
    A = np.r_[map(expa, range(0,m+1))].transpose()
    Ai = np.linalg.pinv(A)

    return Ai[k]

def smooth(x, y, size=5, order=2, deriv=0):

    if deriv > order:
        raise Exception, "deriv must be <= order"

    n = len(x)
    m = size

    result = np.zeros(n)

    for i in xrange(m, n-m):
        start, end = i - m, i + m + 1
        f = sg_filter(x[start:end], order, deriv)
        result[i] = np.dot(f, y[start:end])

    if deriv > 1:
        result *= math.factorial(deriv)

    return result

def plot(t, plots):
    n = len(plots)

    for i in range(0,n):
        label, data = plots[i]

        plt = py.subplot(n, 1, i+1)
        plt.tick_params(labelsize=8)
        py.grid()
        py.xlim([t[0], t[-1]])
        py.ylabel(label)

        py.plot(t, data, 'k-')

    py.xlabel("Time")

def create_figure(size, order):
    fig = py.figure(figsize=(8,6))
    nth = 'th'
    if order < 4:
        nth = ['st','nd','rd','th'][order-1]

    title = "%s point smoothing" % size
    title += ", %d%s degree polynomial" % (order, nth)

    fig.text(.5, .92, title,
             horizontalalignment='center')

def load(name):
    f = open(name)    
    dat = [map(float, x.split(' ')) for x in f]
    f.close()

    xs = [x[0] for x in dat]
    ys = [x[1] for x in dat]

    return np.array(xs), np.array(ys)

def plot_results(data, size, order):
    t, pos = load(data)
    params = (t, pos, size, order)

    plots = [
        ["Position",     pos],
        ["Velocity",     smooth(*params, deriv=1)],
        ["Acceleration", smooth(*params, deriv=2)]
    ]

    create_figure(size, order)
    plot(t, plots)

if __name__ == '__main__':
    data = sys.argv[1]
    size = int(sys.argv[2])
    order = int(sys.argv[3])

    plot_results(data, size, order)
    py.show()

以下是各种参数的一些示例图(使用您提供的数据)。

3pt 平滑,2 次多项式 7pt 平滑,2 次多项式 11pt 平滑,二次多项式 11pt 平滑,4 次多项式 11pt 平滑,10 次多项式

请注意,随着窗口大小的增加,加速度的分段常数性质如何变得不那么明显,但可以通过使用高阶多项式在一定程度上恢复。当然,其他选项涉及两次应用一阶导数滤波器(可能具有不同的阶数)。另一件应该很明显的事情是这种类型的 Savitzky-Golay 过滤,因为它使用窗口的中点,随着窗口大小的增加越来越多地截断平滑数据的末端。有多种方法可以解决该问题,但以下论文中描述了其中一种更好的方法:

PA Gorry,通过卷积 (Savitzky–Golay) 方法进行的一般最小二乘平滑和微分,肛门。化学。62 (1990) 570–573。谷歌

同一作者的另一篇论文描述了一种比示例代码中的直接方法更有效的平滑非均匀数据的方法:

PA Gorry,通过卷积方法对非均匀间隔数据进行一般最小二乘平滑和微分,Anal。化学。63 (1991) 534–536。谷歌

最后,Persson 和Strang在这方面还有一篇值得一读的论文

PO Persson, G. Strang,Savitzky–Golay 和 Legendre 滤波器的平滑,通讯。比较。财务 13 (2003) 301–316。pdf链接

它包含更多的背景理论,并专注于选择窗口大小的错误分析。

您可能应该只做与此问题和答案相同的操作, .

编辑:删除了对二维的引用;该代码实际上只使用了一个(另一个是时间变量)。

但是,您的时间样本似乎不是均匀分布的。这更是一个问题。