在另一篇文章卡尔曼滤波原理介绍及算法实现介绍了卡尔曼滤波器的设计和代码实现，但是滤波后，可以看出，由于状态转移方程不准确，最后把系统响应特性的成分也滤除了，无法应用于导纳控制。本文将介绍有限冲激响应数字滤波器(finite impulse response)的设计及代码实现，FIR主要优点和特点如下：

• （1）FIR数字滤波器可以设计成具有严格线性相位特性的，同时又可以具有所需要的的幅度特性；
• （2）由于FIR数字滤波器的单位脉冲响应是有限长的，且全零点结构，这种滤波器一定是稳定的；
• （3）由于任何非因果有限长系统，只要经过一定的延时，都可以转化为因果系统，固FIR数值滤波器总可以设计为因果系统；
• （4）FIR数字滤波器可以利用FFT来进行信号的滤波处理，可以显著提高计算效率。

FIR数字滤波器的基本思想：
FIR数字滤波器设计的基本思想是在给定所求的理想数字滤波器频率响应H d(ejw)的前提下，设计一个有限脉冲响应滤波器h(n),使其频率特性 H(ejw)逼近H d(ejw)，从而使h(n)逼近hd(n)。整个设计是在时域进行的，关键技术是 对脉冲响应的加窗技术，在设计中也需要进行时域-频域之间的转换，故FIR数字滤波器的窗函数设计又称为傅里叶级数法。

FIR设计方法

窗函数设计法

（1）由理想数字滤波器的频率响应求其理想的单位脉冲响应
期望数字滤波器频率特性，低通、高通、带通等都在该函数中设计

期望数字滤波器单位脉冲脉冲响应，其一般是无限的，所以需要下文加窗口截断

（2）对其进行加窗处理

加窗处理后，其脉冲响应变成有限的，因果的。

（3）加窗后的单位脉冲响应的频率特性
如果选择单位矩形窗，则加窗处理后的单位脉冲响应为

根据傅里叶变换频域卷积性质，可以获得加窗后的频率响应

期望低通滤波器和矩形窗口的频域响应图和单位脉冲响应图如下所示

（4）对比分析
合理选择N和窗函数，可以使加窗后的滤波器频率响应接近于期望滤波器的频率响应

频率抽样法

与窗函数法不同，频率抽样法设计FIR滤波器的设计思路是在频域对期望滤波频率响应进行等间距抽样，得到

由H(k)经过IDFT变换，可以唯一确定有限长系列h(n)，直接完成滤波器设计。

常见窗口函数

• （1）矩形窗
• （2）三角窗
• （3）汉宁窗
• （4）哈明窗
• （5）布莱克曼窗

导纳控制六维力案例

在导纳控制中，机械臂末端六维力期望变换一般是低频的，但是会有高频的噪音，我们设计一个低通滤波器，实现对六维力传感器数据进行滤波，获得期望力的六维力数据。
设计期望频率响应

加矩形窗

选取参数滤波器参数
(1) N=20
(2) wc=0.32

代码实现

本文采用Python实现代码

#!/usr/bin/python
#-*-coding:utf-8-*-
#本文档用于实现有限滤波器FIR算法
#程序员：陈永*
#版权：哈尔滨工业大学(深圳)
#日期：2020.10.5

import numpy as np
import matplotlib.pyplot as plt

#====有限脉冲响应滤波器=======#
class FIRFilter(object):
    def __init__(self, wc, N):
        # 截断频率
        self.wc = wc
        # 输入阶数
        self.N = N

    def set_input_init(self, x0):
        #获取变量维数
        self.m = len(x0)
        self.x0 = list(x0)

    def set_rectangle_filter(self):
        # 计算矩形窗加窗后脉冲响应函数
        self.h_r = np.zeros(self.N)
        alpha = (self.N-1)/2.0
        for i in range(self.N):
            if(abs(i - alpha)<pow(10, -6)):
                self.h_r[i] = self.wc/np.pi
            else:
                self.h_r[i] = np.sin(self.wc*(i - alpha))/(np.pi*(i-alpha))
        self.h_r = self.h_r/np.sum(self.h_r)
        # 设计输入初值
        self.X_r = list(np.zeros([self.N, self.m]))
        for i in range(self.N):
            self.X_r[i] = self.x0

    def set_hanning_filter(self):
        # 计算汉宁加窗后脉冲响应函数
        self.h_n = np.zeros(self.N)
        alpha = (self.N - 1) / 2.0
        for i in range(self.N):
            if (abs(i - alpha) < pow(10, -6)):
                self.h_n[i] = 0.5*(1-np.cos(2*i*np.pi/(self.N-1)))*self.wc / np.pi
            else:
                self.h_n[i] = 0.5*(1-np.cos(2 * i * np.pi/(self.N-1))) * \\
                              np.sin(self.wc * (i - alpha)) / (np.pi * (i - alpha))

        self.h_n = self.h_n / np.sum(self.h_n)
        # 设计输入初值
        self.X_n = list(np.zeros([self.N, self.m]))
        for i in range(self.N):
            self.X_n[i] = self.x0

    #矩形窗滤波器
    def rectangle_filter(self, x):
        #更新输入
        self.X_r.append(x)
        del self.X_r[0]

        #计算输出
        X = np.array(self.X_r)
        y = np.dot(X.T, self.h_r)
        return y

    #汉宁窗滤波器
    def hanning_filter(self, x):
        # 更新输入
        self.X_n.append(x)
        del self.X_n[0]

        # 计算输出
        X = np.array(self.X_n)
        y = np.dot(X.T, self.h_n)
        return y

def FIR_filter():
    #从上文有限冲击响应滤波器建立六维力滤波器
    wc = np.pi/10.0
    N = 20
    FIR_filt = FIRFilter(wc, N)
    F0 = np.full(6, 10)
    FIR_filt.set_input_init(F0)
    FIR_filt.set_hanning_filter()
    FIR_filt.set_rectangle_filter()

    #获取滤波后的数据：本文制造一组虚拟数据
    n = 6
    num = 1000
    T = 0.01
    t = np.linspace(0, (num - 1)*T, num)
    F = np.zeros([num, n])
    for i in range(n):
        F[:, i] = 10 + 1*np.sin(np.pi/2*t) + 1*np.random.randn(num)

    #带入滤波器
    Fr_filt = np.zeros([num, n])
    Fn_filt = np.zeros([num, n])
    for i in range(num):
        Fr_filt[i, :] = FIR_filt.rectangle_filter(F[i, :])
        Fn_filt[i, :] = FIR_filt.hanning_filter(F[i, :])

    #绘画函数,仅考虑第一维
    plt.figure(1)
    plt.plot(t, F[:, 0], label=\'Fx\', color=\'b\')
    plt.plot(t, Fr_filt[:, 0], label=\'Fr_filt\', color=\'r\')
    plt.plot(t, Fn_filt[:, 0], label=\'Fn_filt\', color=\'g\')
    plt.title(\"FIR_filter\")
    plt.xlabel(\"t/s\")
    plt.ylabel(\"f/N\")
    plt.legend()
    plt.show()

if __name__ == \"__main__\":
    FIR_filter()

对加窗后的单位脉冲响应做归一化处理，使滤波后的幅值与原信息号基本相等，两种窗口的滤波效果都优于卡尔曼滤波器的效果，达到导纳控制需求。