卡尔曼滤波器及其在云台控制中的应用

0. 前言 背景

在5月份结束的ICRA RoboMaster人工智能挑战赛中,大疆创新机器人队几乎横扫所有参赛队伍,取得优异成绩。

自动机器人凝聚了算法组在定位、导航、决策、目标识别等各个方面无数心血。作为一个嵌入式软件组的成员,今天我来谈谈我在云台控制和目标跟随上做的一些微小的工作。

与此同时,RoboMaster对抗赛已经进行到第四年,越来越多的队伍都开始思考:在机械结构和电路软件日趋稳定成熟的今天,怎样和其他队伍拉开差距,怎样才能有效的提高队伍的比赛表现?大家不约而同的选择了往自动化、智能化方向发展。

观看过RoboMaster分区赛的小伙伴们肯定会对很多强队的超高命中率影响深刻,例如哈工大和华工的英雄机器人,能够准确命中运动中的敌对目标,显然他们是利用了视觉识别进行辅助瞄准。距离RoboMaster全国总决赛还有三个星期,希望这篇文章能在最后关头给参赛队伍在自瞄功能的完善上提供一些参考和帮助。

本文主要介绍在实现自动瞄准跟随的时候,需要怎么对控制信号做处理以达到准确平滑的追踪效果,主要是嵌入式软件的工作,默认大家的机械结构已经稳定可靠,目标检测和识别算法都已经搞定了。

本文来自于ICRA自动机器人的云台控制工作的个人笔记,包含有问题分析、优化思路、卡尔曼滤波的简单介绍和仿真,以及实际对比情况。定位入门,注重实用性,因此理论推导部分从简,并且尽量用通俗易懂的方式进行介绍。文末会附上卡尔曼滤波的matlab仿真代码和stm32平台实现的c代码供参考。

话不多说,进入正题

1. 云台原始控制方式和存在的问题

1.1 曾经的控制方式

硬件平台:nVidia TX2+STM32开发板

相机固定在底盘上,TX2处理图像数据后,直接得到目标在相机坐标中的位置,通过串口发送给STM32开发板 ,单片机接收后将角度信息作为云台位置环的给定,然后利用双环PID算法(位置环、速度环)对云台进行控制。

1.2 存在的问题

1. 信息损失:

视觉算法得到的原始信息是目标的xyz坐标和时间t,然而直接发送给单片机的是转换后的pitch和yaw角度,从四个独立的变量变成两个变量,自然会有信息损失,例如没有了深度和时间信息。

2. 帧率限制:

受相机帧率和串口通讯速率限制,TX2发送的控制指令的最高频率只有50Hz,而且由于相机采集画面不稳定导致丢帧,所以实际的指令延迟会更大而且不稳定。实际采集的一段原始的yaw轴控制信号如下所示,可以明显的看到信号呈散点状,间隔时间约为28ms,但是并不一致:


如果单片机接收到这样的位置信号不进行任何处理就直接执行,由于单片机执行云台控制逻辑的周期为1ms,因此会带来零阶保持的效果,所以对于底层来说,位置信号是这样阶梯状的:

很显然直接用这样的信号作为云台的位置环的设定值,云台运行起来角度就会阶梯状跳跃,直观表现就是不连贯、卡顿。以前的控制方案是选择将云台PID控制器的增益减小,以达到“柔和”的效果,然而这样带来的显著缺陷就是响应速度非常慢。

3. 没有做预测:

由于相机采集数据、视觉算法执行、数据传输、滤波、PID控制,再到实际云台系统的固有延迟,还要考虑子弹的飞行时间,每个环节都存在不同程度的延时,如果不对云台角度做一定的预测,只能始终跟在目标的后面,无法做到实时跟踪并且命中运动目标。

2. 应对思路和策略

总体思路是嵌入式端对TX2发送过来的云台控制指令进行优化(估计、平滑),尽量保证平滑且低延迟,然后根据目标速度、距离等信息做出角度超前量的预测,加到一起后作为云台角度的目标值,最后调用PID执行云台控制过程。

2.1 插值+低通滤波

要求控制信号由跳动、离散的信号变成连续、平滑的信号,最直观的想法就是对信号插值、然后滤波,由于实时系统的要求,只能做零阶保持插值,即插值成阶梯状的信号。

在考虑滤波算法的时候,首先想到的是低通滤波,为了达到较好的平滑的效果,必须要将滤波器的截止频率设置的很低,这样就会带来严重的滞后,不可行。

MATLAB仿真结果如下(截止频率为10Hz),延迟大概有150ms:

2.2 卡尔曼滤波

单纯的想让信号平滑不可行,必须要加入一定的预测,这就引入了卡尔曼滤波器,因为卡尔曼滤波器是带有预测功能的估计算法(estimate的三个境界:smoothing->filtering->prediction 观点来源:

现代的先进控制理论先进在哪里?如何评价 PID 控制器份额在 95% 条件下稳定性逊色的先进控制系统?www.zhihu.com图标

3. 卡尔曼滤波器介绍

这部分内容的理论部分主要是翻译自University of North Carolina at Chapel Hill的

《An Introduction to the Kalman Filter》

视频截图来自于MATLAB官方制作的视频教程

Understanding Kalman Filters

这两个都是非常通俗易懂的入门教程,强烈推荐。

3.1 卡尔曼滤波的意义

卡尔曼滤波器是以最小均方误差(minimizes the mean of the squared error)为目的而推导出的几个递推数学等式,用于高效的估计过程的状态(provides an efficient computational (recursive) means to estimate the state of a process)。

简单来说,卡尔曼滤波器就是一个带有预测功能的低通滤波器。

3.2 预备知识

一直说卡尔曼滤波器是一个状态估计器,那什么是状态呢,这里就不得不提一下状态和状态空间。

状态是指在某一时刻,可以表征系统特征或者行为的数。

状态空间分析方法是现代控制理论的基础,是用于描述多输入多输出、含有(非)线性关系的系统时引入的系统描述方法。简单来说,你用几个变量表示系统状态,然后写几个方程理清楚它们的关系,然后你就构成了一个状态空间。状态空间分析法是一种时域分析方法。

卡尔曼滤波器的前提条件是系统是线性高斯系统,线性高斯系统需要从两个方面的理解,

线性:运动方程、观测方程是线性的

高斯:系统噪声服从零均值的高斯分布

综合来说也就是说高斯分布的噪声在经过状态转移之后仍然保持高斯分布。

放几张来自MATLAB的视频截图帮助理解

高斯分布
线性变换
非线性变换

由上图可以看到,如果系统是非线性,高斯分布经过变换之后就会不是高斯分布了,这也就超出了卡尔曼滤波的工作范围。为了能够在非线性系统中继续使用卡尔曼滤波,就需要做一些巧妙的变换,例如在局部近似线性化(EKF)、利用离散点投影拟合高斯分布(UKF),这些都是后话。

3.3 计算过程和参数影响

3.3.1 设定

下面进入数学计算过程:

假设一个线性离散随机过程:

x_k = Ax_{k-1} +Bu_{k-1}+w_{k-1}

测量值为

z_k=Hx_k+v_k

H是状态变量到测量值的转换矩阵

w和v分别是过程噪声和测量噪声,假设两者相互独立,且都是正态分布的白噪声(高斯白噪声),满足以下条件:

p(w) \sim N(0,Q)

p(v)\sim N(0,R)

  • Q、R分别是过程噪声和测量噪声的协方差矩阵(对角线上是方差,其他是元素的协方差),假设它们是定值。R可以根据传感器数据大概估算出来,Q是依赖模型的准确度,需要调试。
  • \hat{x}^-_k 先验状态估计(预测值)
  • \hat{x}_k 后验状态估计(估计值)
  • P^-_k 先验误差协方差
  • P_k 后验误差协方差

3.3.2 递推过程

  1. 计算预测值、预测值和真实值之间的误差协方差矩阵
    \hat{x}^-_k=A\hat{x}_{k-1}+Bu_{k-1}
    P^-_{k}=AP_{k-1}A^T+Q
  2. 计算卡尔曼增益,得到估计值
    K_k= P^-_{k}H^T(H P^-_{k}H^T+R)^{-1}
    \hat{x}_k=\hat{x}^-_k+K_k(z_k-H\hat{x}^-_k)
  3. 计算估计值和真实值之间误差协方差矩阵,为下次递推做准备
    P_k=(I-K_kH)P^-_k

将计算过程连起来,形成了一个循环,这就是卡尔曼滤波算法的递推过程

再次引用matlab的视频截图来帮助理解

\hat x_{k-1}​ 经过状态转化方程得到估计值 \hat{x}^-_k​y_k​ 是测量值,根据两者的置信度得到最优估计值 \hat{x}_k

3.3.3 参数对估计效果的影响

A、B、H根据模型来确定。

测量噪声大,R需要设置的较大;模型不准确,Q需要增大。

\frac{Q}{R} 决定了滤波器的稳态频率响应,Q↑,R↓,都会导致卡尔曼增益K变大,从而使得估计值倾向于测量值(包含更多的高频噪声信号)

P变大也会导致卡尔曼增益变大,但是很快会迭代至稳定值,因此P的初始值只会对前几个周期的估计值有影响。

3.3.4 卡尔曼滤波与互补滤波的关系

在MATLAB上跑仿真的时候,将实验数据导入卡尔曼滤波器进行计算,经过十几次迭代后,能够观察到误差协方差矩阵P和卡尔曼增益K都变成定值了,而且导入不同的实验数据也会由同样的结果。

经过长时间的迭代,状态已经收敛到真值附近,而且过程协方差矩阵也已经收敛为

P = \lim_{k\rightarrow \infty } P_{k|k-1}

这是一个常数矩阵,所以卡尔曼增益变成了:

K =K_g = P H_k^T \left (H_k P H_k^T+R_k\right)^{-1}

这也是一个常数而且对于已知的系统,这个常矩阵可以求极限求出来,卡尔曼滤波器就简化成两个等式,不需要递推运算了,此时就等同于一个互补滤波器了,那岂不是我们推导了半天的卡尔曼滤波算法都没有意义了?

显然不是这样的,卡尔曼滤波器的增益是动态调整的,它可以保证计算结果尽快收敛到真实值附近,给出最优估计。而当初始状态设定与真实值有很大误差的时候,互补滤波就会收敛很慢或者根本无法收敛。

3.4 卡尔曼滤波器仿真和对比

3.4.1 仿真设计

按照机器人的实际工作情况,假设真值是幅度20,周期1s的的正弦波,受信号采集过程限制,假设信号采样频率为50Hz,并且叠加了幅度为1的噪声。执行器的频率为1kHz。

使用MATLAB产生幅度为20,周期为1s的正弦波信号作为真实值。先采样至50Hz(模拟图像采集的帧率,丢帧导致的停顿),叠加幅度为1的正态分布的噪声信号,采用一阶保持的方法插值到1kHz作为测量值(模拟单片机对数据的过采样),分别使用卡尔曼滤波和低通滤波对信号进行处理,观察曲线判断波动和延迟情况,计算两种滤波结果和真实值之间的欧式距离来计算偏差大小。

3.4.2 滤波器的实现

1.(一维)卡尔曼滤波器

\hat{x}^-_k=\hat{x}_{k-1}

P^-_{k}=P_{k-1}+Q

K_k=\frac{P^-_k}{P^-_k+R}

\hat{x}=\hat{x}^-_k+K_k(z_k-\hat{x}^-_k)

P_k=(1-K_k)P^-_k

全部是标量计算,调节Q/R的比值来控制滤波效果。

2. 低通滤波器

使用FDATool,选择切比雪夫Ⅱ型IIR滤波器,根据需求设置截止频率和阶数,然后生成object文件,可以方便的设置滤波器参数,然后被主程序调用。

3.4.3 对比结果

1. 对比卡尔曼滤波和低通滤波

设置噪声幅度为1(5%),经过调试,卡尔曼滤波器设置为Q/R=1/2000,低通滤波器截止频率为20Hz,结果如下:


无论是延迟还是平滑度,卡尔曼滤波器的效果都好于低通滤波。计算了滤波值和真值的的欧氏距离作为偏差的判定。

2. 对比不同噪声的效果

调整噪声幅度为3(15%),不改变滤波器参数,再次对比效果:

可以看出,在噪声放大3倍之后,低通滤波的结果波动非常大,几乎完全不可信了,而卡尔曼滤波器受到的影响不大,滤波结果仍然比较平滑,因此可以认为卡尔曼滤波器对噪声不是特别敏感。

4. 云台控制方案实战

在经过一通理论分析和仿真对比后,是时候将卡尔曼滤波器搬到云台系统中进行测试了。我采取的方法是利用TX2采集敌对目标移动的数据,然后导入到MATLAB中设计滤波器。

4.1 使用角度控制量

首先是在原来的控制方案上进行改进:将相机固定在底盘上,TX2计算pitch和yaw角度,底层对控制量进行卡尔曼滤波后作为云台设定值来控制云台。与低通滤波器结果对比如下:

可以看出经过卡尔曼滤波之后,数据平滑了许多,而且没有产生很大的延迟,比较好的满足了云台控制的需求。

4.2 坐标系和运动学模型的选择

由于相机采集的画面不太稳定,难免会产生识别目标的位置发生跳动的情况,这些异常数据需要事先剔除才能达到比较好的滤波效果。最简单的方法就是设置一个阈值,当两帧数据差值大于阈值就认为该数据异常。但是这种方案在直接使用角度作为控制量的条件下很难实现,因为敌对目标以同样的速度运行,在不同距离进行观测得到的角度变化率是不一样的,无法设置阈值为定值。

正确的做法应该是根据目标的运动学特性来对采集的数据进行处理,这就需要得到目标在绝对坐标系下的坐标。其次还可以建立更加准确的模型。

参考自动驾驶中常用于车辆状态估计的车辆运动模型,主要包含以下几类:

只考虑直线运动

  • 恒定速度模型(Constant Velocity, CV)
  • 恒定加速度模型(Constant Acceleration, CA)

如果考虑考虑物体的转弯

  • 恒定转率和速度模型(Constant Turn Rate and Velocity,CTRV)
  • 恒定转率和加速度模型(Constant Turn Rate and Acceleration,CTRA)

由于麦轮底盘可以左右平移,并不存在恒定转率的前提;再加上我们识别的是装甲板,在机器人发生转向的时候,目标会发生切换,因此在设定运动学模型时候不需要考虑转弯。为了简化运算,我们选用了CV模型。

状态空间  \vec{x} = \{x,y,v_x,v_y\}^T

状态转移函数

状态转移矩阵R= \begin{bmatrix} 1 & 0 & \Delta t & 0\\ 0 & 1 & 0 & \Delta t\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ \end{bmatrix}

在验证方案的初期,我们让底盘静止不动,因此机器人自身坐标系和绝对坐标系就等价了。

4.3 观测量的选择

在确定了坐标系和运动模型后,我对观测量的选择也做出了一些测试。

由前面的讨论可以看出,卡尔曼滤波器的观测值和状态量没有直接的关系,例如我可以用一个雷达测量角度然后转化成距离的状态量。对我们的机器人云台系统来说,实际所采用的传感器只有一个——相机,对图像信息经过诸如PNP解算之类的算法得到距离和长度信息,因此相机可以看成一个坐标传感器,但是如果我对距离做差分就可以得到速度信息,这样我们就获得了一个额外的速度传感器了。那么自己计算的速度信息到底有没有用呢,还是用实战的方法来检测一下。

1.观测量选择为x,y坐标

只输入x,y,由卡尔曼滤波器自行估算速度

观测矩阵为

H=\begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ \end{bmatrix}\\

x方向滤波结果如下,图上方的两条是位置曲线,下方是速度曲线

2.观测量包含全部状态

自行计算v_x,v_y,根据加速度限制对跳变进行抑制,将全部状态输入至卡尔曼滤波器

观测矩阵为单位对角矩阵,测量误差矩阵设置为 R= \begin{bmatrix} 2000 & 0 & 0 & 0\\ 0 & 2000 & 0 & 0\\ 0 & 0 & 10000 & 0\\ 0 & 0 & 0 & 10000\\ \end{bmatrix}\\

x方向结果如下


由两张图可以看出,在速度波动范围大致相同的情况下,第二种方案的延迟明显较小。

第二种方案由于自行计算了目标在x、y方向的速度,然后利用已有的先验知识对速度跳变进行抑制,有效的排除了噪声数据对滤波结果的影响,所以得到的效果较好。

4.4 运动预测

到目前为止,我们已经利用卡尔曼滤波将云台的控制信号优化到一个令人满意的地步了,但是在实际测试过程中发现在打击动态目标时的命中率还是很低,具体表现是子弹始终落在移动目标后方 ,究其原因就是因为没有考虑到系统的延迟。

最简单的运动预测就是认为目标是匀速移动,用速度乘上系统的延迟时间,就得到了超前量,然后加到云台角度的设定上。在我们刚才设置的运动模型中,包含有速度状态,因此可以直接从卡尔曼的输出结果中调用。

实际平台跑出来的结果如下所示

蓝色线为加了简单的运动预测后结果,可以看出确实做到了超前原始控制信号的,但是在目标转向出不可避免的产生了超调,这也是运动预测最容易产生的后果,所以实际效果还需要进行取舍和优化。

4.5 相机转移到云台上

前面所有的测试都是把相机放在底盘上进行的,这是为了保持相机相对稳定,能够很简单的获取目标相对于地面的真实运动情况,但是在实战中,不仅要求能够准确命中目标,还要能够有效的躲避敌人的攻击,例如最常用的扭腰,在这种情况下,底盘的运动幅度甚至会远大于云台;此外,为了保证相机视角的迅速切换,也要求相机被安装在云台上。

相机跟随云台一起运动,最本质的区别就是去掉了相机相对于地面静止不动的前提。根据之前的推论,想要准确的跟踪目标就需要在绝对坐标系下进行计算,因此需要利用云台的姿态和与云台上相机获取的目标坐标进行处理,得到其在绝对坐标系下的位置和速度。但是该方案需要对机器人自身做出准确迅速的定位,难以实现。

一种可行的方案是将相机坐标系下计算出的目标位置(角度量)加到云台自身实际角度上(考虑到延迟,需要使用几十ms前的历史数据),然后进行卡尔曼滤波,再超前推算一小段时间得到云台的目标位置。实测该方案能够取得比较好的控制效果。

附两段测试视频:

第一个是四月份的第一版效果,没有加入射击时机的控制

第二个是接近完成的版本,敌我双方互相移动的情况下打击效果几乎一致。

https://www.zhihu.com/video/996642206844821504https://www.zhihu.com/video/996651228453101568

5. 卡尔曼滤波器在STM32平台的实现和MATLAB仿真代码

由于卡尔曼滤波运算过程涉及到大量的矩阵运算,有多种实现方法:

  1. 调用开源的矩阵运算库,效率较低
  2. 将矩阵运算拆开成基本的四则运算,扩展性很差,例如改变矩阵阶数就没办法用了,而且在遇到例如求逆运算的时候比较麻烦
  3. 调用STM32自带的DSP库中的矩阵运算功能,可以根据需求自行添加,简单高效。

具体使用方法很简单,开启stm32的FPU,然后把需要用到的运算库文件包含到工程里面,然后就可以调用相应的函数进行运算了。

附录1:在stm32平台上运行的二阶卡尔曼滤波器

/* second-order kalman filter on stm32 */
#include "stm32f4xx_hal.h"
#include "arm_math.h"

#define mat         arm_matrix_instance_f32 
#define mat_init    arm_mat_init_f32
#define mat_add     arm_mat_add_f32
#define mat_sub     arm_mat_sub_f32
#define mat_mult    arm_mat_mult_f32
#define mat_trans   arm_mat_trans_f32
#define mat_inv     arm_mat_inverse_f32

typedef struct
{
  float raw_value;
  float filtered_value[2];
  mat xhat, xhatminus, z, A, H, AT, HT, Q, R, P, Pminus, K;
} kalman_filter_t;

typedef struct
{
  float raw_value;
  float filtered_value[2];
  float xhat_data[2], xhatminus_data[2], z_data[2],Pminus_data[4], K_data[4];
  float P_data[4];
  float AT_data[4], HT_data[4];
  float A_data[4];
  float H_data[4];
  float Q_data[4];
  float R_data[4];
} kalman_filter_init_t;

void kalman_filter_init(kalman_filter_t *F, kalman_filter_init_t *I)
{
  mat_init(&F->xhat,2,1,(float *)I->xhat_data);
  ...
  mat_init(&F->HT,2,2,(float *)I->HT_data);
  mat_trans(&F->H, &F->HT);
}

float *kalman_filter_calc(kalman_filter_t *F, float signal1, float signal2)
{
  float TEMP_data[4] = {0, 0, 0, 0};
  float TEMP_data21[2] = {0, 0};
  mat TEMP,TEMP21;

  mat_init(&TEMP,2,2,(float *)TEMP_data);
  mat_init(&TEMP21,2,1,(float *)TEMP_data21);

  F->z.pData[0] = signal1;
  F->z.pData[1] = signal2;

  //1. xhat'(k)= A xhat(k-1)
  mat_mult(&F->A, &F->xhat, &F->xhatminus);

  //2. P'(k) = A P(k-1) AT + Q
  mat_mult(&F->A, &F->P, &F->Pminus);
  mat_mult(&F->Pminus, &F->AT, &TEMP);
  mat_add(&TEMP, &F->Q, &F->Pminus);

  //3. K(k) = P'(k) HT / (H P'(k) HT + R)
  mat_mult(&F->H, &F->Pminus, &F->K);
  mat_mult(&F->K, &F->HT, &TEMP);
  mat_add(&TEMP, &F->R, &F->K);

  mat_inv(&F->K, &F->P);
  mat_mult(&F->Pminus, &F->HT, &TEMP);
  mat_mult(&TEMP, &F->P, &F->K);

  //4. xhat(k) = xhat'(k) + K(k) (z(k) - H xhat'(k))
  mat_mult(&F->H, &F->xhatminus, &TEMP21);
  mat_sub(&F->z, &TEMP21, &F->xhat);
  mat_mult(&F->K, &F->xhat, &TEMP21);
  mat_add(&F->xhatminus, &TEMP21, &F->xhat);

  //5. P(k) = (1-K(k)H)P'(k)
  mat_mult(&F->K, &F->H, &F->P);
  mat_sub(&F->Q, &F->P, &TEMP);
  mat_mult(&TEMP, &F->Pminus, &F->P);

  F->filtered_value[0] = F->xhat.pData[0];
  F->filtered_value[1] = F->xhat.pData[1];

  return F->filtered_value;
}

附录2:一阶卡尔曼滤波与低通滤波器对比

%% 一阶卡尔曼滤波与低通滤波器对比
% 初始化参数
ts = 0.001;                 % 采样时间
delta_t  = 0.02;            % 实际信号帧率
noise_mag = 1;              % 设置噪声幅度为1
t = 0:ts:2-ts;
t2 = 0:delta_t:2-delta_t;
N = length(t);              % 序列的长度
sz = [1,N];                 % 信号需开辟的内存空间大小 
x = 20*sin(2*pi*t);         % 真实位置
noise = noise_mag*randn(1,N*ts/delta_t);              % 测量白噪声
z = interp1 (t2,20*sin(2*pi*t2)+noise,t,'previous');  % 信号叠加噪声后重新采样
Q = 1;                      % 假设建立的模型噪声方差
R = 2000;                   % 位置测量方差估计

A=1;
B=0;
H=1;

n=size(Q);  
m=size(R);

% 分配空间
xhat=zeros(sz);         % 后验估计
P=0;                    % 后验方差估计  
xhatminus=zeros(sz);    % 先验估计
Pminus=zeros(n);        % 先验方差估计
K=zeros(n(1),m(1));     % Kalman增益
I=eye(n);

% 估计的初始值都为默认的0,即P=[0 0;0 0],xhat=0
for k = 2:N
    % 时间更新过程
    xhatminus(:,k) = A*xhat(:,k-1)+B;
    Pminus= A*P*A'+Q;
    
    % 测量更新过程
    K = Pminus*H'/( H*Pminus*H'+R );
    xhat(:,k) = xhatminus(:,k)+K*(z(k)-H*xhatminus(:,k));
    P = (I-K*H)*Pminus;
end
F = filter_custom;
z_lp = filter(F,z);
x_k = cat(1,x(1:1950),xhat(1:1950));
distance_k = pdist(x_k); % 计算两个向量的欧氏距离作为偏差的判定
x_lp = cat(1,x(1:1950),z_lp(1:1950));
distance_lp = pdist(x_lp);
x_z = cat(1,x(1:1950),z(1:1950));
distance_z = pdist(x_z);

figure;
plot(t,x(1,:),'g-');
hold on
plot(t,z);
plot(t,xhat(1,:),'r-')
plot(t,z_lp,'k-');
legend('truth',['measurement ',num2str(distance_z)], ['kalman filter ',num2str(distance_k)], ['lowpass ',num2str(distance_lp)]);
xlabel('time(s)');

function Hd = filter_custom
% Chebyshev Type II Lowpass filter designed using FDESIGN.LOWPASS.

% All frequency values are in Hz.
Fs = 1000;  % Sampling Frequency

N     = 10;  % Order
Fstop = 20;  % Stopband Frequency
Astop = 80;  % Stopband Attenuation (dB)

% Construct an FDESIGN object and call its CHEBY2 method.
h  = fdesign.lowpass('N,Fst,Ast', N, Fstop, Astop, Fs);
Hd = design(h, 'cheby2');
end

编辑于 2018-07-01

文章被以下专栏收录