深入浅出讲解卡尔曼滤波（附Matlab程序）

深入浅出讲解卡尔曼滤波（附Matlab程序）

发布时间：2019-07-17 10:28:18 浏览：3171

简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。

在学习卡尔曼滤波器之前，首先看看为什么叫“卡尔曼”。跟其他著名的理论（例如傅立叶变换，泰勒级数等等）一样，卡尔曼也是一个人的名字，而跟他们不同的是，他是个现代人！学过控制的应该都知道，卡尔曼是现代控制理论的奠基人!

卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。

为了给大家能讲解清楚卡尔曼滤波器，我们找到两篇关于卡尔曼滤波器非常好的文章：

第一篇来源于CSDN博客，为大家详细的讲解了卡尔曼的原理及应用，算作“深入”

第二篇来源于知乎，用一个简单的例子，通俗易懂的讲解了卡尔曼滤波，算作“浅出”

此外，关于卡尔曼滤波的仿真程序在EETOP论坛里有很多，大家可以登录论坛后搜索“卡尔曼”来查找。这里我们给大家提供了一个Matlab仿真程序，可以通过点击左下角的“阅读原文”进入论坛下载。

第一篇

原文地址：http://blog.csdn.net/lanbing510/article/details/40936343

1.卡尔曼滤波器的介绍（Introduction to the Kalman Filter）为了可以更加容易的理解卡尔曼滤波器，首先应用形象的描述方法来讲解，然后我们结合其核心的5条公式进行进一步的说明和探索。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。在介绍他的5条公式之前，先让我们来根据下面的例子做个直观的解释。

假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（Gaussian Distribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。

好了,现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。

假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。 由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance来判断。因为Kg=5^2/(5^2+4^2)，所以Kg=0.6098，我们可以估算出k时刻的实际温度值是：23+0.6098*(25-23)=24.22度。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。 现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.22度）的偏差。算法如下：

((1-Kg)*5^2)^0.5=3.12。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的3.12就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。 就是这样，卡尔曼滤波器就不断的把covariance递归，从而估算出最优的温度值。他运行的很快，而且它只保留了上一时刻的covariance。上面的Kg，就是卡尔曼增益（Kalman Gain）。他可以随不同的时刻而改变他自己的值，是不是很神奇！ 下面就要言归正传，讨论真正工程系统上的卡尔曼。

2. 卡尔曼滤波器算法（The Kalman Filter Algorithm）

在这一部分，我们就来描述源于Dr Kalman 的卡尔曼滤波器。下面的描述，会涉及一些基本的概念知识，包括概率（Probability），随即变量（Random Variable），高斯或正态分配（Gaussian Distribution）还有State-space Model等等。但对于卡尔曼滤波器的详细证明，这里不能一一描述。

首先，我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程（Linear Stochastic Difference equation）来描述，我们结合下面PPT截图进行说明：

上两式子中，x(k)是k时刻的系统状态，u(k)是k时刻对系统的控制量。A和B是系统参数，对于多模型系统，他们为矩阵。y(k)是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。q(k)和r(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise)，他们的covariance分别是Q，R（这里我们假设他们不随系统状态变化而变化）。

对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是最优的信息处理器。先给出KF算法的流程和五个核心更新方程如下：KF算法

五个更新方程为：

编写公式不方便，所以写成了PDF然后做了截图粘在了下面，下面就上面的例子和五个核心的公式对Kalman算法进行下说明：

就这样，算法就可以自回归的运算下去。

看到这聪明的同学可能已经看出来了，问道卡尔曼增益为什么会是第三步中那样求，现在只大致说一下原理，具体推到比较复杂，有兴趣的同学可以参考这文献去推一推。还记得前面我们说的误差协方差矩阵$P_k$么，即求第k次最优温度的误差协方差矩阵，对应于上例中的3和3.12....这些值。看下面PPT，我们最小化P即可得到卡尔曼增益K，对应上例求解K只最小化最优温度值的偏差，即最小化P(K)：

我们由第四步可以看出，k时刻系统的最优温度值=k-1时刻状态估计值（由上一状态的最优温度值加上过程误差）+带卡尔曼增益权值项的偏差。如果观测误差远远大于估计误差，那么K就很小，k时刻的预测值约等于k时刻的状态估计值，如果对i时刻的状态估计值误差远远大于观测误差，此时相应的q较大，K较大，i时刻的状态估计值更倾向于观察的数据。

卡尔曼滤波器的原理基本描述就完成了，希望能帮助大家理解这这5个公式，其算法可以很容易的用计算机的程序实现。下面，我会用程序举一个实际运行的例子。

3.简单例子（A Simple Example）这里我们结合第二第三节，举一个非常简单的例子来说明卡尔曼滤波器的工作过程。所举的例子是进一步描述第二节的例子，而且还会配以程序模拟结果。根第二节的描述，把房间看成一个系统，然后对这个系统建模。当然，我们见的模型不需要非常地精确。我们所知道的这个房间的温度是跟前一时刻的温度相同的，所以A=1。没有控制量，所以u(k)=0。因此得出：x(k|k-1)=x(k-1|k-1) ……… (6)式子（2）可以改成：P(k|k-1)=P(k-1|k-1) +Q ……… (7)因为测量的值是温度计的，跟温度直接对应，所以H=1。式子3，4，5可以改成以下：X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1)) ……… (8)Kg(k)= P(k|k-1) / (P(k|k-1) + R) ……… (9)P(k|k)=（1-Kg(k)）P(k|k-1) ……… (10)

第二篇

考虑轨道上的一个小车，无外力作用，它在时刻t的状态向量只与相关：（状态向量就是描述它的t=0时刻所有状态的向量，比如：[速度大小5m/s, 速度方向右, 位置坐标0]，反正有了这个向量就可以完全预测t=1时刻小车的状态）那么根据t=0时刻的初值，理论上我们可以求出它任意时刻的状态。当然，实际情况不会这么美好。这个递推函数可能会受到各种不确定因素的影响（内在的外在的都算，比如刮风下雨地震，小车结构不紧密，轮子不圆等等）导致并不能精确标识小车实际的状态。我们假设每个状态分量受到的不确定因素都服从正态分布。现在仅对小车的位置进行估计请看下图：t=0时小车的位置服从红色的正态分布。

根据小车的这个位置，我们可以预测出t=1时刻它的位置：

分布变“胖”了，这很好理解——因为在递推的过程中又加了一层噪声，所以不确定度变大了。为了避免纯估计带来的偏差，我们在t=1时刻对小车的位置坐标进行一次雷达测量，当然雷达对小车距离的测量也会受到种种因素的影响，于是测量结果告诉我们，小车t=1时的位置服从蓝色分布：

好了，现在我们得到两个不同的结果。前面有人提过加权，Kalman老先生的牛逼之处就在于找到了相应权值，使红蓝分布合并为下图这个绿色的正态分布（啰嗦一句，这个绿色分布均值位置在红蓝均值间的比例称为Kalman增益(比如下图中近似0.8)，就是各种公式里的K(t)）

你问为什么牛逼？绿色分布不仅保证了在红蓝给定的条件下，小车位于该点的概率最大，而且，而且，它居然还是一个正态分布！正态分布就意味着，可以把它当做初值继续往下算了！这是Kalman滤波能够迭代的关键。最后，把绿色分布当做第一张图中的红色分布对t=2时刻进行预测，算法就可以开始循环往复了。你又要问了，说来说去绿色分布是怎么得出的呢？其实可以通过多种方式推导出来,我们课上讲过的就有最大似然法、Ricatti方程法，以及上面参考文献中提及的直接对高斯密度函数变形的方法，这个不展开说了。

基于FPGA的宽频超声波电源频率跟踪系统设计

苏文虎，陈 迅

（江苏科技大学 电子信息学院，江苏 镇江212003）

针对传统超声波电源无法驱动及锁频不同谐振频率段的换能器，实现不了宽频域内的锁相和频率跟踪的问题，设计了一种基于FPGA的具有自动频率搜索与跟踪、动态匹配不同谐振频率换能器的宽频域超声波电源。根据换能器的阻抗特性曲线，设计出动态步长的宽频域频率搜索方法，快速跟踪到换能器的谐振频率，并根据反馈电路的电压电流相位差，实时调整输出频率，锁定整个系统工作在谐振状态。实验结果表明，设计的宽频域超声波电源频率搜索快、跟踪准，动态匹配换能器适应性好。

频率搜索；频率跟踪；宽频超声波电源

中图分类号： TN86

文献标识码： A

DOI： 10.16157/j.issn.0258-7998.2017.03.014

中文引用格式： 苏文虎，陈迅. 基于FPGA的宽频超声波电源频率跟踪系统设计[J].电子技术应用，2017，43(3)：59-62.

英文引用格式： Su Wenhu，Chen Xun. Research on frequency tracking system of wide-band ultrasonic power supply based on FPGA[J].Application of Electronic Technique，2017，43(3)：59-62.

0 引言

超声波焊接、清洗、检测技术具有广泛的应用前景[1]。然而超声波系统谐振频率漂移、跟踪滞后等问题并没有得到完全解决。目前研制的超声波设备多为超声波电源与其配套的换能器工作，单个超声波电源无法驱动及锁频不同谐振频率段的换能器，实现不了宽频域内的频率跟踪和锁相[2]。因此，研制一款具有频率自动跟踪并能动态匹配不同谐振频率换能器的宽频超声波电源具有重要的应用价值[3]。

本文采用XILINX ZYNQ系列的FPGA(XC7Z0201CLG484I)作为主控制器，设计了一款具有宽频域内频率自动搜索、跟踪、动态匹配换能器的超声波电源。输出频率20~40 kHz，可以驱动在该频率范围内的不同应用类型换能器，具有宽频域的广泛适用性。

1 超声波电源系统构成

超声波电源由整流电路、逆变电路、匹配电路、反馈电路和主控制电路组成。系统结构框图如图1所示。主控制器FPGA集成了一个双核ARM Cortex-A9 处理器资源(Processing System，PS)和一个传统的现场可编程门阵列(Field-Programmable Gate Array，FPGA)逻辑资源(Programmable Logic，PL)。FPGA的PL单元主要完成频率搜索、频率跟踪、电压电流鉴相、SPWM生成、直接数字式频率合成器（DDS）等功能；PS处理器完成界面显示、输入参数处理等功能。匹配电路由高频变压器和改进T型匹配网络组成，起着变阻调谐的作用，使负载呈纯阻性，提高电源的效率。

DDS可简化为由相位累加器和ROM正弦波存储表组成。在系统时钟的控制下，更新频率控制字，该频率控制字对应着ROM正弦波表的地址，而该地址存储的内容就是正弦波的某个合成点对应的幅值，在下一个系统时钟下，相位累加器的输出再次增加一个频率控制字，对应着改变波形存储的地址，从而生成所合成波形的下一个幅值点，直到相位累加器溢出后形成一个完整的正弦波[4-5]。该正弦波的频率随着频率控制字的改变而改变，其输出频率为：

式中，N为相位累加器位长，fclk为系统时钟，Fword为频率控制字。

DDS输出正弦波信号与SPWM生成模块内部产生的高频三角载波信号进行调制，输出脉冲宽度按照正弦波规律变化的PWM波，控制逆变电路输出频率按照DDS输出的正弦波频率改变。

系统通电后，首先根据反馈电流有效值I_in进行频率搜索，在20 kHz~40 kHz频域内快速搜索到换能器的谐振频率，并记录谐振频率时的最大电流值I_max作为电流阈值，接着启动频率跟踪程序，根据反馈电路的电压U、电流I相位差，锁定输出频率在电压电流相位一致的频率点，同时PL还实时监测反馈电流值与I_max之间的大小，作为判断系统是否处于谐振状态的一个辅助条件。一旦系统更换换能器或者负载突变导致系统失谐，反馈电流值将远小于I_max，此时PL将选择频率搜索程序，重新搜索到换能器的谐振频率后，转到频率跟踪进行相位锁定。

2 频率搜索

超声波电源输出频率与换能器谐振频率相等时，电源工作效率最高，系统工作最稳定[6]。实验表明，换能器工作在谐振频率Fs时，其负载流过的电流值最大[7-8]，因此可以通过检测换能器的电流值实现谐振频率搜索。

图2绘制了谐振频率为22 kHz和32.32 kHz的换能器在20 kHz~40 kHz频段内的阻抗特性曲线。由图2可知，换能器在谐振频率附近阻抗会发生巨大变化，即电流值变化明显；而在远离谐振频率时，阻抗变化不明显，从而可知其电流变化微弱。基于换能器的这一阻抗特性，在FPGA平台上设计了基于电流最大值的频率搜索法。频率搜索的FPGA实现如图3所示。

系统通电后由频率搜索模块产生20 kHz输出频率所需的频率控制字，在系统时钟控制下，比较当前反馈的电流有效值I_in与上一周期反馈的电流值，若二者差值小于设定的阈值Ith，说明此时负载电流变化不明显，系统频率远离换能器的谐振频率，此时以设定的大步长step1改变DDS的频率控制字；当二者差值大于Ith，说明此时的系统频率已接近换能器的谐振频率，频率控制字将以设定的小步长step2微调，直到输出频率达到40 kHz，频率搜索功能完成，此时记录搜索过程中的电流最大值I_max，并输出电流最大值对应的频率控制字。频率搜索的软件流程图如图4所示。图中各变量的注释如下：

(1)Freq：搜索频率范围(20 kHz~40 kHz)

(2)F_word_rem：谐振频率控制字

(3)I_in：输入电流有效值

(4)I_max：电流最大值

(5)step、step1、step2：代表不同步长

(6)Ith：设定阈值

(7)det_I：相邻采样周期的电流差值。

系统上电后初始化频率Freq为20 kHz，在每个采样周期下，比较输入电流I_in与I_max的关系。如果输入电流大于I_max，将输入电流值赋给I_max，并记录此时的频率点赋值给F_word_rem，同时比较相邻采样周期内的电流值以产生det_I，随后比较det_I与Ith的关系。如果det_I大于Ith，说明此时系统的输出频率已接近换能器的谐振频率点，输出频率Freq将以小步长step2递变；反之说明输出频率远离换能器的谐振频率，为加快搜索速度，应以大步长step1在宽频域改变输出频率。当完成频域内搜索之后，输出频率字为电流值最大时刻对应的谐振频率点F_word_rem，频率搜索程序完成。

图5为频率搜索程序的ModelSim仿真图。光标位置代表电流最大值点，即换能器的谐振频率点。在换能器的谐振频率附近，输出的频率字以小步长step=10变化，逐步搜索到谐振频率点，并记录下谐振频率为20 260以及此时的电流最大值3 471。图5表明，依据换能器阻抗特性曲线设计的变步长搜索方法满足了设计要求。

3 频率跟踪

系统工作在谐振状态时，换能器等效负载呈纯阻性，电压电流相位差近似为0。因此可以通过检测电压电流相位差判断电路是否处于谐振状态[9]。频率跟踪时，FPGA对反馈的电压电流信号进行相位比较，生成电压电流相位差信号，根据相位差信号改变频率控制字的输出，从而改变系统输出频率，直到反馈的电压电流相位差趋近0，锁定整个系统工作在换能器的谐振频率上。频率跟踪的FPGA实现如图6所示。

反馈电路输出的Phase_I、Phase_U二路信号分别代表换能器的电流电压相位信号，二者经过数字鉴相后，输出电流电压相位差值，由脉宽计数器记录下相位差大小，结合给定步长step改变DDS的频率控制字，即改变逆变电路的输出频率，从而改变Phase_I与Phase_U的相位差，直到系统稳定在谐振频率点上。频率跟踪软件流程图如图7所示。

在搜索到谐振频率F_word_rem后，系统启动频率跟踪程序，将F_word_rem的值送给Freq作为系统频率控制字输出，由FPGA采集反馈的电压电流信号。当检测到电流相位超前电压相位时，以步长step减小频率控制字Freq，反之增加频率控制字频率，改变系统的输出频率，直到电压电流相位差在设定的裕度。

图8为频率跟踪程序的ModelSim仿真图，光标位置为电压电流零相位差的频率点。由图8可知，在电压相位超前电流相位时，输出频率控制字Freq在谐振频率F_word_rem的基础上，以step=1 Hz的步长增加，对应着驱动逆变电路的PWM信号频率以相应步长递增，锁定系统在谐振频率点；反之当电流相位超前电压相位时，输出频率随着频率字Freq以相应步长递减，直到反馈的电压电流相位一致。

4 实验结果

借助于MATLAB R2014a对设计的系统进行仿真，以20 kHz换能器为例，对反馈的电压电流信号进行相位差分析，波形如图9所示。

图9(a)为偏离谐振频率点的电压电流波形，二者存在一定相位差，此时启动频率跟踪程序，改变输出频率使电压电流相位差趋近0，波形如图9(b)所示，电压电流相位差几乎为0。实验结果表明，设计的宽频域超声波电源频率跟踪效果良好，换能器工作在谐振状态。

5 结论

在已有超声波电源的分析基础上，研制了一款基于XILINX ZYNQ 7000系列FPGA的宽频域超声波电源。在分析了换能器阻抗特性曲线的特性后，设计了变步长的频率搜索方法，快速搜索到换能器的谐振频率点，根据电压电流相位差锁定系统输出频率在换能器的谐振频率。所设计的超声波电源频率搜索快、跟踪准，并可以动态匹配不同谐振频率的换能器。该款FPGA芯片内部集成的大量资源，降低了外围电路的复杂度，使硬件设计变得简单稳定。实验结果证明，设计的宽频超声波电源可以驱动换能器工作在谐振状态，换能器工作稳定、温度低，在频率跟踪方面表现良好，并能达到设计要求。

参考文献

[1] Zhou Wei，Zhang Dongquan.Research and development of ultrasonic frequency dual closed-loop control[C].International Conference on Digital Manufacturing and Automation，2012.

[2] 李长有.超声波电源只能频率跟踪系统应用研究[J].电力电子技术，2014，48(3)：35-38.

[3] Dong Huijuan，Wu Jian，Zhang Guangyu，et al.An improved phase-locked loop method for automatic resonance frequency tracing based on static capacitance broadband compensation for a high-power ultrasonic transducer[J].IEEE Transactions on Ultrasonics，Ferroelectrics and Frequency Control，2012，59(2)：205-210.

[4] 张兴红，蔡伟，邱磊，等.基于FPGA与DDS技术的可调超声波驱动电源设计[J].仪表技术与传感器，2015(6)：84-87.

[5] 胡武林，曹彪，杨景卫.超声波焊接电源频率跟踪与调节[J].电焊机，2014，44(7)：47-51.

[6] 唐新星.基于频率自动跟踪与振幅恒定控制的超声电源的研制[D].北京：北京交通大学，2014.

[7] 屈百达.基于DSP的超声换能器频率跟踪系统[J].压电与声光，2014，36(2)：297-301.

[8] SHIANG-HWUA YU，YI-FEI HSIEH，PEI-YING LAI，et al．FPGA-based resonant-frequency-tracking power amplifier for ultrasonic transducer[C].International Conference on Applied Electronics，2015.

[9] 刘晓光，刘平峰，蒋晓明，等.基于FPGA的超声波焊接电源频率跟踪研究[J].自动化与信息工程，2015，36(4)：39-43.

相关问答

在进行典型环节的模拟研究时,我们需要掌握基本的控制理论知识,如传递函数、频率响应、稳定性分析等。同时,还需要熟练掌握MATLAB等仿真工具的使用方法,能...

>>symssinx:>>[sinx,3]。ans=[sinx,3]。>>diff(ans)。ans=[1,0]。MATLAB的基本数...

这个专业就业前景很好,是不错的专业。专业层次:专科(高职)专业学制:三年专业介绍:核心课程:电路理论、模拟电子技术、数字电...这个专业就业前景很好,是...

可以啊,但是得配合下位机单片机硬件系统。首先下位机加入外部温度传感器,一般用到的温度传感器有ds18b20,和温湿度传感器dh111,然后配合单片机开发板,51单片...

[回答]然后通过基于坐标变换的方法实现电机电流电压在不同坐标系下的计算。最后通过MATLAB/Simulink实现永磁同步电机的仿真模型,对不同匝数下永磁同步电...

中国大数据产业受宏观政策环境、技术进步与升级、数字应用普及等众多利好因素影响,2018年整体规模达到4384.5亿元,较2017年同比增长24%。我国大数据行业细分领...

1,建立小车数学模型,最好能用matlab仿真,然后整定pid参数,2.使用高速控制器,最好dsp或者stm32,51系列也行,但是较慢,3,可以使用摄像头代替光电传感器来...1...

VR软件工程师:掌握C/C++/Java语言,熟悉算法实现、Matlab、UI设计等,熟练使用Unity3d,掌握3DMax,Maya等建模软件。VR硬件工程师:掌握C/C++语言,懂电子电......

机器人是一个综合性很强的系统,而且和它的服务类型也有关系,比如服务型机器人,工业机器人等。服务型机器人又细分很多,比如教育机器人,客服机器人,家庭机器...

人工智能确实可以编程。近日,据外媒TNW报道,GoogleAutoML系统居然自己写了一串机器学习代码,其效率竟然超过了专业的研发工程师。这让我们人类的优越感何...