3-卡尔曼滤波-1详解

1、1 Kalman Filtering2 背景介绍：背景介绍： Kalman，匈牙利数学家。 卡尔曼滤波器源于他的博士论 文和1960年发表的论文A New Approach to Linear Filtering and Prediction Problems（线 性滤波与预测问题的新方法）。3 估计原理和卡尔曼滤波估计原理和卡尔曼滤波1. 状态估计原理状态估计原理 2. 为什么要用状态估计理论为什么要用状态估计理论 3. 经典控制理论与现代控制理论经典控制理论与现代控制理论 4. 什么是卡尔曼滤波什么是卡尔曼滤波5.卡尔曼滤波器的软硬件实现卡尔曼滤波器的软硬件实现6.卡尔曼滤波器的应用卡尔曼

2、滤波器的应用 4 1.状态估计原理状态估计原理 状态估计是卡尔曼滤波的重要组成部分。一状态估计是卡尔曼滤波的重要组成部分。一般来说，根据观测数据对随机量进行定量推断就般来说，根据观测数据对随机量进行定量推断就是估计问题，特别是对动态行为的状态估计，它是估计问题，特别是对动态行为的状态估计，它能实现实时运行状态的估计和预测功能。比如对能实现实时运行状态的估计和预测功能。比如对飞行器状态估计。飞行器状态估计。 5 状态估计对于了解和控制一个系统具有重状态估计对于了解和控制一个系统具有重要意义，所应用的方法属于统计学中的估计理论。要意义，所应用的方法属于统计学中的估计理论。最常用的是最小二乘估计，线

3、性最小方差估计、最常用的是最小二乘估计，线性最小方差估计、最小方差估计、递推最小二乘估计等。其他如风最小方差估计、递推最小二乘估计等。其他如风险准则的贝叶斯估计、最大似然估计、随机逼近险准则的贝叶斯估计、最大似然估计、随机逼近等方法也都有应用。等方法也都有应用。 6 受噪声干扰的状态量是个随机量，不可能测得精受噪声干扰的状态量是个随机量，不可能测得精确值，但可对它进行一系列观测，并依据一组观测值，确值，但可对它进行一系列观测，并依据一组观测值，按某种统计观点对它进行估计。按某种统计观点对它进行估计。 使估计值尽可能准确地接近真实值，这就是最优使估计值尽可能准确地接近真实值，这就是最优估计。估计

4、。 真实值与估计值之差称为估计误差。真实值与估计值之差称为估计误差。 若估计值的数学期望与真实值相等，这种估计称若估计值的数学期望与真实值相等，这种估计称为无偏估计。为无偏估计。 7 卡尔卡尔曼曼提出的递推最优估计理论，采用状态空提出的递推最优估计理论，采用状态空间描述法，算法采用递推形式，卡尔曼滤波能处理间描述法，算法采用递推形式，卡尔曼滤波能处理多维和非平稳的随机过程。多维和非平稳的随机过程。 卡尔曼滤波理论的提出，克服了威纳滤波理论卡尔曼滤波理论的提出，克服了威纳滤波理论的局限性，使其在工程上得到了广泛的应用，尤其的局限性，使其在工程上得到了广泛的应用，尤其在控制、制导、导航、通讯等现代

5、工程方面。在控制、制导、导航、通讯等现代工程方面。8关于维纳滤波和卡尔曼滤关于维纳滤波和卡尔曼滤维纳滤波和卡尔曼滤波都是解决线性滤波和预测问题的方法维纳滤波和卡尔曼滤波都是解决线性滤波和预测问题的方法，并且都是以均方误差最小为准则的，在平稳条件下两者的，并且都是以均方误差最小为准则的，在平稳条件下两者的稳态结果是一致的。但是它们解决问题的方法有很大区别。稳态结果是一致的。但是它们解决问题的方法有很大区别。1. 维纳滤波是根据全部过去观测值和当前观测值来估计信号维纳滤波是根据全部过去观测值和当前观测值来估计信号的当前值，因此它的解形式是系统的传递函数的当前值，因此它的解形式是系统的传递函数H(z

6、)或单位脉或单位脉冲响应冲响应h(n) ，因此更常称这种系统为最佳线性过滤器或滤波，因此更常称这种系统为最佳线性过滤器或滤波器；卡尔曼滤波是用当前一个估计值和最近一个观测值来估器；卡尔曼滤波是用当前一个估计值和最近一个观测值来估计信号的当前值，它是用状态方程和递推的方法进行估计的计信号的当前值，它是用状态方程和递推的方法进行估计的，它的解形式是以估计值，它的解形式是以估计值 (常常是状态变量值）。因此更常常常是状态变量值）。因此更常称这种系统为线性最优估计器或滤波器。称这种系统为线性最优估计器或滤波器。2. 维纳滤波只适用于平稳随机过程，卡尔曼滤波就没有这个维纳滤波只适用于平稳随机过程，卡尔曼

7、滤波就没有这个限制。限制。93. 维纳过滤中信号和噪声是用相关函数表示的，因此设计维纳维纳过滤中信号和噪声是用相关函数表示的，因此设计维纳滤波器要求已知信号和噪声的相关函数。卡尔曼过滤中信号和滤波器要求已知信号和噪声的相关函数。卡尔曼过滤中信号和噪声是状态方程和量测方程表示的，因此设计卡尔曼滤波器要噪声是状态方程和量测方程表示的，因此设计卡尔曼滤波器要求已知状态方程和量测方程求已知状态方程和量测方程(当然，相关函数与状态方程和量当然，相关函数与状态方程和量测方程之间会存在一定的关系测方程之间会存在一定的关系)。卡尔曼过滤方法看来似乎比维纳过滤方法优越，它用递推法计卡尔曼过滤方法看来似乎比维纳过

8、滤方法优越，它用递推法计算，不需要知道全部过去的数据，从而运用计算机计算方便，算，不需要知道全部过去的数据，从而运用计算机计算方便，而且它可用于平稳和不平稳的随机过程而且它可用于平稳和不平稳的随机过程(信号信号)，非时变和时变，非时变和时变的系统。的系统。10但从发展历史上来看维纳滤波的思想是但从发展历史上来看维纳滤波的思想是40年代初提出来的，年代初提出来的，1949年正式以书的形式出版。卡尔曼滤波到年正式以书的形式出版。卡尔曼滤波到60年代初才提出年代初才提出来，它是在维纳滤波的基础上发展起来的，虽然如上所述它来，它是在维纳滤波的基础上发展起来的，虽然如上所述它比维纳滤波方法有不少优越的地

9、方，但是最佳线性滤波问题比维纳滤波方法有不少优越的地方，但是最佳线性滤波问题是由维纳滤波首先解决的，维纳滤波的物理概念比较清楚，是由维纳滤波首先解决的，维纳滤波的物理概念比较清楚，也可以认为卡尔曼滤波仅仅是对最佳线性过滤问题提出的一也可以认为卡尔曼滤波仅仅是对最佳线性过滤问题提出的一种新的算法。种新的算法。当被估计的状态变量很多时，卡尔曼滤波运算量以及存储量当被估计的状态变量很多时，卡尔曼滤波运算量以及存储量将会变得很大，因为需要存储和更新状态预测误差的自相关将会变得很大，因为需要存储和更新状态预测误差的自相关矩阵。这也是他的缺点。矩阵。这也是他的缺点。wiener滤波用于标量滤波，而滤波用于

10、标量滤波，而kalman滤波可用于矢量滤波滤波可用于矢量滤波，所以应用也更广些。，所以应用也更广些。112 2为什么要用状态估计理论为什么要用状态估计理论 在许多实际问题中，由于随机过程的存在，常常不在许多实际问题中，由于随机过程的存在，常常不能直接获得系统的状态参数，需要从夹杂着随机干扰的能直接获得系统的状态参数，需要从夹杂着随机干扰的观测信号中分离出系统的状态参数。观测信号中分离出系统的状态参数。 例如，飞机在飞行过程中所处的位置、速度等状态例如，飞机在飞行过程中所处的位置、速度等状态参数需要通过雷达或其它测量装置进行观测，而雷达等参数需要通过雷达或其它测量装置进行观测，而雷达等测量装置也

11、存在随机干扰，因此在观测到飞机的位置、测量装置也存在随机干扰，因此在观测到飞机的位置、速度等信号中就夹杂着随机干扰，要想正确地得到飞机速度等信号中就夹杂着随机干扰，要想正确地得到飞机的状态参数是不可能的，只能根据观测到的信号来估计的状态参数是不可能的，只能根据观测到的信号来估计和预测飞机的状态，这就是估计问题。和预测飞机的状态，这就是估计问题。12 从观测到的信号中估计出状态的估值，并且希从观测到的信号中估计出状态的估值，并且希望估值与状态的真值越小越好，即要求有：望估值与状态的真值越小越好，即要求有： 成立；成立； 因此存在最优估计问题，这就是卡尔曼滤波。因此存在最优估计问题，这就是卡尔曼滤

12、波。 卡尔曼滤波的最优估计需满足以下三个条件：卡尔曼滤波的最优估计需满足以下三个条件： 无偏性，即估计值的均值等于状态的真值；无偏性，即估计值的均值等于状态的真值； 估计的方差最小；估计的方差最小； 实时性。实时性。min)()( txtx13 从以上分析可以看出卡尔曼滤波就是在有随从以上分析可以看出卡尔曼滤波就是在有随机干扰和噪声的情况下，以线性最小方差估计方机干扰和噪声的情况下，以线性最小方差估计方法给出状态的最优估计值，卡尔曼滤波是在统计法给出状态的最优估计值，卡尔曼滤波是在统计的意义上给出最接近状态真值的估计值。因此，的意义上给出最接近状态真值的估计值。因此，卡尔曼滤波在空间技术、测轨

13、、导航、拦截与通卡尔曼滤波在空间技术、测轨、导航、拦截与通讯等方面获得了广泛的应用。讯等方面获得了广泛的应用。143 3经典控制理论与现代控制理论经典控制理论与现代控制理论 经典控制理论只适应与单输入经典控制理论只适应与单输入单输出的线性定常系单输出的线性定常系统，研究方法是传递函数。传递函数在本质上是一种频率统，研究方法是传递函数。传递函数在本质上是一种频率法，要靠各个频率分量描述信号。因此，频率法限制了系法，要靠各个频率分量描述信号。因此，频率法限制了系统对整个过程在时间域内进行控制的能力，所以经典控制统对整个过程在时间域内进行控制的能力，所以经典控制理论很难实现实时控制。同时，经典控制理

14、论也很难实现理论很难实现实时控制。同时，经典控制理论也很难实现最优控制。最优控制。 反 馈 量 控 制 器Gc 输 入 量 + - 受 控 对 象Go 反 馈 环 节H 输 出 量 u r c 控 制 量 偏 差 n 扰 动 量 b 经典控制系统的组成经典控制系统的组成15 由于经典控制理论的上述局限性，随着科学由于经典控制理论的上述局限性，随着科学技术的发展，特别是空间技术和各类高速飞行器技术的发展，特别是空间技术和各类高速飞行器的快速发展，要求控制高速度、高精度的受控对的快速发展，要求控制高速度、高精度的受控对象，控制系统更加复杂，要求控制理论解决多输象，控制系统更加复杂，要求控制理论解决

15、多输入多输出、非线性以及最优控制等设计问题。这入多输出、非线性以及最优控制等设计问题。这些新的控制要求经典控制理论是无法解决的。些新的控制要求经典控制理论是无法解决的。16 现代控制理论是建立在状态空间基础上的，现代控制理论是建立在状态空间基础上的，它不用传递函数，而是用状态向量方程作为基本工它不用传递函数，而是用状态向量方程作为基本工具，因此可以用来分析多输入具，因此可以用来分析多输入多输出、非线性以多输出、非线性以及时变复杂系统的研究。及时变复杂系统的研究。 现代控制理论本质上是时域法，信号的描述现代控制理论本质上是时域法，信号的描述和传递都是在时间域进行，所以现代控制理论具有和传递都是在

16、时间域进行，所以现代控制理论具有实现实时控制的能力。由于采用了状态空间法，现实现实时控制的能力。由于采用了状态空间法，现代控制理论有利于设计人员根据给定的性能指标设代控制理论有利于设计人员根据给定的性能指标设计出最优的控制系统。计出最优的控制系统。 17 卡尔曼滤波控制系统结构图卡尔曼滤波控制系统结构图 由于系统的状态由于系统的状态x是不确定的，卡尔曼滤波器的是不确定的，卡尔曼滤波器的任务就是在有随机干扰任务就是在有随机干扰w和噪声和噪声v的情况下给出系统的情况下给出系统状态状态x的最优估算值的最优估算值 ，它在统计意义下最接近状，它在统计意义下最接近状态的真值态的真值x，从而实现最优控制，从

17、而实现最优控制u( )的目的。的目的。 x x 18 4.4.什么是卡尔曼滤波：什么是卡尔曼滤波： 卡尔曼滤波是在线性最小方差估计的基卡尔曼滤波是在线性最小方差估计的基础上，提出的在数学结构上比较简单的而且础上，提出的在数学结构上比较简单的而且是最优线性递推滤波方法，具有计算量小、是最优线性递推滤波方法，具有计算量小、存储量低，实时性高的优点。特别是对经历存储量低，实时性高的优点。特别是对经历了初始滤波后的过渡状态，滤波效果非常好。了初始滤波后的过渡状态，滤波效果非常好。 19 如果系统是高斯的，卡尔曼滤波是最优的如果系统是高斯的，卡尔曼滤波是最优的最小均方误差估计；最小均方误差估计； 如果系

18、统不是高斯的，卡尔曼滤波是线性如果系统不是高斯的，卡尔曼滤波是线性最小均方误差估计，是最优的线性估计器。最小均方误差估计，是最优的线性估计器。 卡尔曼滤波是以最小均方误差为估计的最卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法。佳准则，来寻求一套递推估计的算法。20 基本思想是：采用信号与噪声的状态空基本思想是：采用信号与噪声的状态空间模型，利用前一时刻的估计值和现时刻的间模型，利用前一时刻的估计值和现时刻的观测值来更新对状态变量的估计，求现在时观测值来更新对状态变量的估计，求现在时刻的估计值。它适合于实时处理和计算机运刻的估计值。它适合于实时处理和计算机运算。算。 21

19、 卡尔曼滤波的实质是由量测值重构系统卡尔曼滤波的实质是由量测值重构系统的状态向量。它以的状态向量。它以“预测预测实测实测修正修正”的的顺序递推，根据系统的量测值来消除随机干顺序递推，根据系统的量测值来消除随机干扰，再现系统的状态，或根据系统的量测值扰，再现系统的状态，或根据系统的量测值从被污染的系统中恢复系统的本来面目。从被污染的系统中恢复系统的本来面目。 22卡尔曼滤波特点：卡尔曼滤波特点： 卡尔曼滤波是解决状态空间模型估计与预测的卡尔曼滤波是解决状态空间模型估计与预测的有力工具之一，它不需存储历史数据，就能够从一有力工具之一，它不需存储历史数据，就能够从一系列的不完全以及包含系列的不完全以

20、及包含噪声噪声的的测量测量中，估计中，估计动态系动态系统统的状态。的状态。 卡尔曼滤波是一种卡尔曼滤波是一种递归递归的估计，即只要获知上的估计，即只要获知上一时刻状态的估计值以及当前状态的观测值就可以一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。者估计的历史信息。 23 卡尔曼滤波器与大多数我们常用的滤波卡尔曼滤波器与大多数我们常用的滤波器不同之处，在于它是一种纯粹的器不同之处，在于它是一种纯粹的时域时域滤波滤波器，不需要像器，不需要像低通滤波器低通滤波器等等频域频域滤波器那样，滤波器那样，

21、需要在频域设计再转换到时域实现。需要在频域设计再转换到时域实现。245.卡尔曼滤波器的软硬件实现卡尔曼滤波器的软硬件实现 目前目前,卡尔曼滤波器已经有很多不同的实现形式。卡尔曼滤波器已经有很多不同的实现形式。卡尔曼最初提出的形式现在一般称为简单卡尔曼滤卡尔曼最初提出的形式现在一般称为简单卡尔曼滤波器。除此以外波器。除此以外,还有施密特扩展卡尔曼滤波器还有施密特扩展卡尔曼滤波器,信信息滤波器以及平方根滤波器。最常见的卡尔曼滤波息滤波器以及平方根滤波器。最常见的卡尔曼滤波器是锁相环器是锁相环 ，采用，采用FPGA硬件可以实现卡尔曼滤波硬件可以实现卡尔曼滤波器。器。 25硬件实现硬件实现卡尔曼滤波器

22、有良好的滤波效果，但由于其计算量卡尔曼滤波器有良好的滤波效果，但由于其计算量大，当采样率高时，一个采样周期内难以完成计算，大，当采样率高时，一个采样周期内难以完成计算，且计算机的字长有限，使计算中舍入误差和截断误且计算机的字长有限，使计算中舍入误差和截断误差积累、传递，造成数值不稳定，因此用差积累、传递，造成数值不稳定，因此用MCU和和DSP难以实现。难以实现。FPGA可以实现并行计算，它有多可以实现并行计算，它有多个乘法器和累加器并行处理数据，采用个乘法器和累加器并行处理数据，采用FPGA实现实现的卡尔曼滤波器，由于输入和输出数据计算同时进的卡尔曼滤波器，由于输入和输出数据计算同时进行，因此

23、可以大大提高滤波速度。行，因此可以大大提高滤波速度。 26 一般，数字滤波器的一般，数字滤波器的FPGA实现是用实现是用VHDL或或Verilog HDL等硬件描述语言通过编写底层代码实等硬件描述语言通过编写底层代码实现。这种编程方式效率低，难度大。现。这种编程方式效率低，难度大。 利用利用Altera公司公司FPGA的的DSP开发工具开发工具DSP Builder设计卡尔曼滤波器，比基于硬件描述语言的设计卡尔曼滤波器，比基于硬件描述语言的设计设计,周期更短周期更短,设计更容易。设计更容易。 27 基于现场可编程逻辑门阵列基于现场可编程逻辑门阵列FPGA器件和模数器件和模数转换器设计的数据采集

24、系统为硬件平台，进行算法转换器设计的数据采集系统为硬件平台，进行算法设计。设计。基于模块化设计思想，设计时钟分频模块、基于模块化设计思想，设计时钟分频模块、AD转换转换芯片的芯片的FPGA控制模块和卡尔曼滤波模块。卡尔曼控制模块和卡尔曼滤波模块。卡尔曼滤波模块采用滤波模块采用DSP Builder设计，转换成硬件描述设计，转换成硬件描述语言语言VHDL后，应用软件后，应用软件Modelsim、QuartusII进进行仿真并完成硬件验证。行仿真并完成硬件验证。28软件实现软件实现 许多物理进程，如路上行驶的车辆、围绕地许多物理进程，如路上行驶的车辆、围绕地球轨道运转的卫星、由绕组电流驱动的电机轴

25、或球轨道运转的卫星、由绕组电流驱动的电机轴或正弦射頻载波信号，均可用线性系统来近似。线正弦射頻载波信号，均可用线性系统来近似。线性系统是指能用如下两个方程描述的简单进程：性系统是指能用如下两个方程描述的简单进程： 29状态方程：状态方程： 输出方程：输出方程：在上述方程中，在上述方程中，A、B和和C均为矩阵，均为矩阵，k是时间系数，是时间系数，x称称为系统状态，为系统状态，u是系统的已知输入，是系统的已知输入，y是所测量的输出。是所测量的输出。w和和z表示噪音，其中变量表示噪音，其中变量w称为进程噪音，称为进程噪音，z称为测量噪音，称为测量噪音，它们都是向量。它们都是向量。30 卡尔曼滤波的算法流程为：卡尔曼滤波的算法流程为：1.预估计预估计X(k)= F(k,k-1)X(k-1) 2.计算预估计协方差矩阵计算预估计协方差矩阵3.C(k)=F(k,k-1)C(k)F(k,k-1)+T(k,k- 1)Q(k)T(k,k-1)4.Q(k) = U(k)U(k) 5.计算卡尔曼增益矩阵计算卡尔曼增益矩阵6.K(k) = C(k)H(k)H(k)C(k)H(k)+R(k)(-1)7.R(k) = N