Python实现利用三次样条插值进行滤波

最新推荐文章于 2024-05-14 17:57:42 发布

我不是程序猿WWW、

最新推荐文章于 2024-05-14 17:57:42 发布

阅读量1.7k

点赞数

分类专栏：滤波算法文章标签：算法 python

本文链接：https://blog.csdn.net/qq_44905590/article/details/106117826

版权

滤波算法专栏收录该内容

1 篇文章 0 订阅

订阅专栏

本人的自动驾驶小车，需要始终获得较为准确的位置信息。采用了一款国外的室内GPS设备来进行定位，官方给出的设备误差在±2cm ，使用过程中设备误差确实比较小。但是在信号正常的情况下，总是时不时的会出现短暂性的“飞点”，这些“飞点”对我的车影响很大，因为车上没有其他定位传感器，无法进行数据融合，考虑使用简单算法实现数据滤波的功能。

仔细研究了常见的十大滤波算法（滑动均值滤波，限幅滤波之类的），并不能满足我的要求。

根据滑动均值滤波的思路和微分的思想，想到滑动三次样条插值滤波方法。

该方法的基本原理：

对位置坐标点数据进行三次样条拟合，截取一段数据（滑动窗）进行拟合，得出拟合方程
对最后一个拟合方程进行求导，将滑动窗的最后一个坐标点的x 值代入求导后的方程，得出该点的斜率k
根据微分的思想，对下一个点进行预测，根据GPS的更新频率，得出点与点之间的间隔时间Δt (我这里是100Hz,Δt=0.01s），根据正常点数据计算出Δv（我的车Δv大致是 0.37m/s）
根据k、Δt、Δv预测出下一个点的位置。计算公式如下

A=arctant(k)

$x = x _{0}+ cos(A)*\bigtriangleup v * \bigtriangleup t$

$y= y _{0}+ sin(A)*\bigtriangleup v * \bigtriangleup t$

从网站上找到合适的三次样条插值算法，对其进行了修改，来实现我想要的功能。

三次样条插值算法原码网址：https://blog.csdn.net/weixin_43061340/article/details/105211860十分感谢这位大佬，写的十分详细。

最终的python代码如下：

Function函数：

import numpy as np
def runge(y):
    y = np.float32(y)
    return 1 / (1 + y * y)

def spline3_Parameters(x_vec, opt=0):
    # 建立三对角矩阵的 4n 个方程的左边部分
    # parameter为二维数组，用来存放参数，size_of_Interval为区间的个数
    x_new = np.array(x_vec)
    parameter = []
    size_of_Interval = len(x_new) - 1
    i = 1
    # 相邻两区间公共节点处函数值相等的方程，共2n-2个
    while i < len(x_new) - 1:
        data = np.zeros(size_of_Interval * 4)
        data[(i - 1) * 4] = x_new[i] * x_new[i] * x_new[i]
        data[(i - 1) * 4 + 1] = x_new[i] * x_new[i]
        data[(i - 1) * 4 + 2] = x_new[i]
        data[(i - 1) * 4 + 3] = 1
        parameter.append(data)
        data = np.zeros(size_of_Interval * 4)
        data[i * 4] = x_new[i] * x_new[i] * x_new[i]
        data[i * 4 + 1] = x_new[i] * x_new[i]
        data[i * 4 + 2] = x_new[i]
        data[i * 4 + 3] = 1
        parameter.append(data)
        i += 1
    # 左右端点处的函数值。为两个方程, 加上前面的2n-2个方程，一共2n个方程
    data = np.zeros(size_of_Interval * 4)
    data[0] = x_new[0] * x_new[0] * x_new[0]
    data[1] = x_new[0] * x_new[0]
    data[2] = x_new[0]
    data[3] = 1
    parameter.append(data)
    data = np.zeros(size_of_Interval * 4)
    data[(size_of_Interval - 1) * 4] = x_new[-1] * x_new[-1] * x_new[-1]
    data[(size_of_Interval - 1) * 4 + 1] = x_new[-1] * x_new[-1]
    data[(size_of_Interval - 1) * 4 + 2] = x_new[-1]
    data[(size_of_Interval - 1) * 4 + 3] = 1
    parameter.append(data)
    # 端点函数一阶导数值相等为n-1个方程。加上前面的方程为3n-1个方程。
    i = 1
    while i < size_of_Interval:
        data = np.zeros(size_of_Interval * 4)
        data[(i - 1) * 4] = 3 * x_new[i] * x_new[i]
        data[(i - 1) * 4 + 1] = 2 * x_new[i]
        data[(i - 1) * 4 + 2] = 1
        data[i * 4] = -3 * x_new[i] * x_new[i]
        data[i * 4 + 1] = -2 * x_new[i]
        data[i * 4 + 2] = -1
        parameter.append(data)
        i += 1
    # 端点函数二阶导数值相等为n-1个方程。加上前面的方程为4n-2个方程。
    i = 1
    while i < len(x_new) - 1:
        data = np.zeros(size_of_Interval * 4)
        data[(i - 1) * 4] = 6 * x_new[i]
        data[(i - 1) * 4 + 1] = 2
        data[i * 4] = -6 * x_new[i]
        data[i * 4 + 1] = -2
        parameter.append(data)
        i += 1

    # 两个附加条件
    # 默认情况：opt = 0，not-a-knot边界条件：左边两端点三阶导相等，右边两端点三阶导也相等
    if opt == 0:
        data = np.zeros(size_of_Interval * 4)
        data[0] = 6
        data[4] = -6
        parameter.append(data)
        data = np.zeros(size_of_Interval * 4)
        data[-4] = 6
        data[-8] = -6
        parameter.append(data)
    # opt = 1，给定左右两端点的一阶导值
    if opt == 1:
        data = np.zeros(size_of_Interval * 4)
        data[0] = 3 * x_new[0] * x_new[0]
        data[1] = 2 * x_new[0]
        data[2] = 1
        parameter.append(data)
        data = np.zeros(size_of_Interval * 4)
        data[-4] = 3 * x_new[-1] * x_new[-1]
        data[-3] = 2 * x_new[-1]
        data[-2] = 1
        parameter.append(data)
    # opt = 2，给定左右两端点的二阶导值
    if opt == 2:
        data = np.zeros(size_of_Interval * 4)
        data[0] = 6 * x_new[0]
        data[1] = 2
        parameter.append(data)
        data = np.zeros(size_of_Interval * 4)
        data[-4] = 6 * x_new[-1]
        data[-3] = 2
        parameter.append(data)

    return parameter


def solution_of_equation(functype, parametes, x, y=0, func=runge, opt=0, lval=0, rval=0):
    # 建立三对角线性方程组并求解，得到各段三次函数的系数并返回
    # functype 表示需要拟合的是给定函数 / 给定点集
    size_of_Interval = len(x) - 1
    result = np.zeros(size_of_Interval * 4)
    i = 1
    if  functype != 'give_nodes':
        raise ValueError("functype should be 'give_nodes' ")

    if functype == 'give_nodes':
        if len(x) != len(y):
            raise ValueError("Expect a node set!")
        while i < size_of_Interval:
            result[(i - 1) * 2] = y[i]
            result[(i - 1) * 2 + 1] = y[i]
            i += 1
        result[(size_of_Interval - 1) * 2] = y[0]
        result[(size_of_Interval - 1) * 2 + 1] = y[-1]

    # 默认情况：opt = 0，not-a-knot边界条件：左边两端点三阶导相等，右边两端点三阶导也相等
    if opt == 0:
        result[-2] = result[-1] = 0
    # opt = 1 或 opt = 2，给定左右两端点的一阶导值 / 二阶导值
    if opt == 1 or opt == 2:
        result[-2] = lval
        result[-1] = rval

    a = np.array(parametes)
    b = np.array(result)
    return np.linalg.solve(a, b)

spline函数：

import Functions as F
def give_nodes(x, y, q,opt=0, lval=0, rval=0):
    if opt == 0 and (lval != 0 or rval != 0):
        raise ValueError('There should be no parameters of lval and rval by default')
    if opt != 0 and opt != 1 and opt != 0 and opt != 2:
        raise ValueError('opt should be 0 or 1 or 2!')

    if opt == 0:
        res = F.solution_of_equation('give_nodes', F.spline3_Parameters(x), x, y)
    if opt == 1:
        res = F.solution_of_equation('give_nodes', F.spline3_Parameters(x, 1), x, y, opt=1, lval=lval, rval=rval)
    if opt == 2:
        res = F.solution_of_equation('give_nodes', F.spline3_Parameters(x, 2), x, y, opt=2, lval=lval, rval=rval)
    k = []
    for i in range(len(x) - 1):
        z='{0:.3f}'.format(3*res[4 * i] * (q ** 2) + 2*res[1 + 4 * i] * q  + res[2 + 4 * i] )
        k.append(z)
    return k[-1]

ertrapolation函数：

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import spline as sp
import numpy as np
import math

def extrapolation(x,y,delta_V=0.37,delta_T=0.01):
    '''
    输入参数：进行拟合的数组x,y ; 速度，时间 默认ΔV = 0.37 ΔT = 0.01
    返回 (x,y)
    '''
    xn = None
    yn = None

    try:
        #计算拟合曲线在临界点的斜率k
        b =x[-1]
        k = sp.give_nodes(x, y, b)
        k = float(k)
        #判断斜率
        if k == 0 and x[-1]>x[-2]:
            xn = x[-1]+delta_V * delta_T
            yn = y[-1]
        elif  k == 0 and x[-1]<x[-2]:
            xn = x[-1] - delta_V * delta_T
            yn = y[-1]
        elif k >= 100 :
            xn = x[-1]
            yn = y[-1] + delta_V * delta_T
        elif k <= -100:
            xn = x[-1]
            yn = y[-1] - delta_V * delta_T

        else:
            A = math.atan(k)
            xn = x[-1]+ math.cos(A)*delta_V * delta_T
            yn = y[-1] + math.sin(A)*delta_V * delta_T        
        

    except np.linalg.LinAlgError:
        pass
    return [xn,yn]

将插值和GPS发送过来的数据做比较,绝对误差大于某个值返回插值数据，小于返回GPS数据。

我不是程序猿WWW、

关注

0
点赞
踩
13

收藏

觉得还不错? 一键收藏
1
评论
Python实现利用三次样条插值进行滤波

本人的自动驾驶小车，需要始终获得较为准确的位置信息。采用了一款国外的室内GPS设备来进行定位，官方给出的设备误差在±2cm ，使用过程中设备误差确实比较小。但是在信号正常的情况下，总是时不时的会出现短暂性的“飞点”，这些“飞点”对我的车影响很大，因为车上没有其他定位传感器，无法进行数据融合，考虑使用简单算法实现数据滤波的功能。仔细研究了常见的十大滤波算法（滑动均值滤波，限幅滤波之类的），并不能满足我的要求。根据滑动均值滤波的思路和微分的思想，想到滑动三次样条插值滤波方法。该方法的基本原理：对
复制链接

扫一扫

专栏目录