ardupilot(EKF)扩展卡尔曼滤波
ardupilot(EKF)扩展卡尔曼滤波
一、初识卡尔曼滤波器
为了描述方便我从网上找了一张卡尔曼滤波器的5大公式的图片。篇幅所限，下图所示的是多维卡尔曼滤波器（因为EKF2是多维扩展卡尔曼滤波器，所以我们从多维说起），为了跟好的理解卡尔曼滤波器可以百度一下，从一维开始。
这5个公式之外还有一个观测模型，根据你实际的观测量来确定，它的主
要作用是根据实际情况来求观测矩阵H。
因为卡尔曼滤波器是线性滤波器，状态转移矩阵A和观测矩阵H是确定的。在维基百科上状态转移矩阵用F表示。在ardupilot EKF2算法中，状态转移矩阵也是用F表示的。下面是维基百科给出的线性卡尔曼滤波器的相关公式。
上述更新（后验）估计协方差的公式对任何增益K k都有效，有时称为约瑟夫形式。为了获得最佳卡尔曼增益，该公式进一步简化为P k|k=(I-K k H k)P k|k-1，它在哪种形式下应用最广泛。但是，必须记住它仅对最小化残差误差的最佳增益有效。
为了使用卡尔曼滤波器来估计仅给出一系列噪声观测过程的内部状态，必须根据卡尔曼滤波器的框架对过程进行建模，这意味着指定一下矩阵：
只要记住一点就行了，卡尔曼滤波器的作用就是输入一些包含噪声的数据，得到一些比较接近真是情况的数据。比如无人机所使用的陀螺仪和加速度计的
读值，他们的读值都是包含噪声的，比如明明真实的角速度是俯仰2°/s,陀螺
仪的读值却是2.5°/s。通过扩展卡尔曼之后的角速度值会变得更加接近2o/s
的真实值，有可能是2.1o/s。
二、扩展卡尔曼滤波器
因为卡尔曼滤波器针对的是线性系统，状态转移模型（说的白话一点就是知道上一时刻被估计量的值，通过状态转移模型的公式可以推算出当前时刻被
估计量的值）和观测模型。注：有的资料显示状态模型中有,有的没有，目前
我也不清楚是为什么，有可能和被估计的对象有关。但看多了你就会发现不管
网上给的公式有怎样的不同，但总体的流程是一样的，都是这5大步骤。我个
人觉得维基百科给的公式较为标准。
因为扩展卡尔曼滤波器（EKF，Extended Kalman filter）的使用场景为非线性系统。所以上面两公式改写为下面所示的样子，我个人的理解是，因为是
非线性系统，所以没有固定的状态转移矩阵和观测矩阵。到这儿为止卡尔曼滤
波器到扩展卡尔曼滤波器的过度就完成了（多说一句，因为传感器的数据采样
是有时间间隔的，算法的运行也是有间隔的，所以本文提到的KF和EKF都是离散型的）。下面是扩展卡尔曼滤波器的相关公式。
有心的你一定发现了，扩展卡尔曼滤波器的状态转移和观测模型中，没有了状态转移矩阵和观测矩阵，但预测和更新过程中，还是要用到和。就
我个人的了解，求和的过程就是对非线性系统线性化。和的求法如下：
看到这两个求和的公式，是不是头都大了。看了半天也不知道是什么意思，光看公式也不知道，这两个矩阵到底该怎么求。我一开始就是这个感觉，什么
雅克比矩阵，什么一阶偏导，听得头都大了（还是大学数学没学好...）。实际上数学家最擅长的事情就是把简单的问题复杂化，把对问题的描述抽象化。抽
象带来的问题就是，公式具有高度的概括性，反映了一系列具有相同特性事物
的关系，简单明了的说明了事物的本质。但抽象化掩盖了细节，使人理解起来
什么十分困难。随后我会结合EKF2的代码作出详细的解释，参照着具体的情况和公式，反复多看几遍，你就会有一个比较清晰的了解。
想要深入了解ardupilot EKF2的原理，一定要把扩展卡尔曼滤波器的这几个公式，背的滚瓜烂熟，这样在后续读代码的时候，就能迅速联想起代码所对
应的过程和公式，这一点很重要。在了解EKF2的初期，并不需要知道扩展卡尔曼滤波器的公式为什么是这样，你只要知道EKF2的每个阶段，用的公式是什么样子的就可以了。事实上我在为EKF2添加观测量并测试成功的两年之后，才逐渐的知道EKF2每一步所使用的公式都是什么。
以自身经验来讲，我觉得对EKF2的了解，需要经历3个阶段。
第一阶段、不知其然
最开始，你想要了解ardupilot姿态解算的（ardupilot的核心代码大致
分为几块，姿态解算（准确的说应该是位姿解算）、姿态控制、位置控制、任
务决策、路径规划等，其中位姿解算是重中之重，其他模块都依赖于位姿解算
模块所提供的准确的姿态、位置信息）相关的代码，看了它类的名字
AP_NavEKF2,你在网上一搜或问了同事、同学、老师，知道了ardupilot使用的是扩展卡尔曼滤波器。你在网上开始查资料，这时你查到的资料鱼龙混杂，有
的说的是扩展卡尔曼滤波器，有的实际上说的是卡尔曼滤波器。在查资料的过
程中你渐渐的知道了四元数、欧拉角、旋转矩阵、旋转向量等名词，但此时的
你还是一头雾水。随着你看的资料越来越多，你逐渐的知道了扩展卡尔曼滤波
器的作用，他的大体过程，他的输入输出是什么。虽然这时你也许还不大清楚
为啥网上查到的公式有的地方长得不大一样，但你发现他们大部分是一样的。
虽然你还不知道和是怎么求得的，对了，你有可能查到的公式里状态转移和观
测矩阵是和（我当时查到的就是这样，这直接导致后面看代码更加的头雾水）。此时，你觉得你对EKF有了一定的了解，开始看代码。你努力的通过代码了解EKF2的程序流程，一开始是一些逻辑代码，你看得有些吃力，但还可以经过一
番鏖战，你终于看到了EKF2的核心代码（在此期间，相同的代码你有可能会看好几遍，而且每一遍的理解都不一样）。当你试着找到那5大公式时，你崩溃了。状态转移矩阵在哪？观测矩阵在哪？是什么？又是什么？怎么还有和？公
式里也没有提到呀。你再往下看，看到了和，你的心情稍微平复了一些，终于
看到两个公式里提到的变量。但这也和理论中的公式相差十万八千里。再往后看，你看到了函数SelectMagFusion，此时的你猜想，这个函数的作用应该是
修正偏航角（Yaw）。你觉得这个函数有可能是一个突破口，于是你在粗略的看
了看这个函数之后，确定了你的想法。之后的你了解了磁罗盘数据的格式和代
表的含义，然后又开始读代码，读着读着，又看到了一大堆的计算，就在你痛
苦不看之时，Kfusion出现在你的眼前。此时的你隐约的感觉到这个函数应该
是修正最终的输出量（偏航角）的地方。在经过无数个日夜，看代码，树立猜想，再看代码，推翻猜想的循环后，你终于知道了这个函数里关键变量的作用，但此时的你还是不知道那一大坨的计算是干什么的，不过多天的思考，你知道
只要找到输入，在修正量一致的情况下，计算过程应该是一致的，你沿着这个
思路，修改了输入，发现输出确实发生了变化。经过无数个日夜的煎熬，终于
送了口气。此时的你沿着这个方向，经过多次的修改、测试，终于实现了自己
想要的结果。得到预想结果的你，高兴极了。既然修改成功了，目标达到了，EKF2里又有一大坨计算，看着都让人头疼，这个事情也可以放放了。毕竟自己
已经是修改过EKF2，ardupilot核心代码的人了。
第二阶段、知其然
当你经历过第一阶段后，你就去了解其他方面的知识了（也有可能是，你
实在看不懂那一坨计算代码是干什么的，拼命的看了一段时间，还是没有任何
进展，这时投入产出比变得很低很低，你觉得不如先去了解下其他方面的知
识）。在学习其他知识的过程中，你还是时常想起EKF2那些你没有弄明白的问题，你时常翻看EKF2相关的代码、之前找到的EKF资料，也时不时的从网上再查看一些新的资料，还有那个matlab脚本文件（GenerateNavFilterEquations.m）。通过注释你知道了，EKF2的代码是从这
个脚本中来的，你在第一阶段中也看过这个脚本，但也没有看出来，EKF2是怎
么通过这个脚本来。也许你对matlab也不是那么熟悉（语法似乎和C++差不多，都是变量、函数，但又感觉差的很多），在看脚本的时候，看得迷迷糊糊，痛
苦的很。所有在你翻看了这些资料后，你又去了解其他的知识了。期间你知道
了ROS、了解了人工智能，知道了python的作用和应用场景，所以你花时间学
习了python。这时你还是时常想着ardupilot EKF2的实现到底是个怎样的过程。在你不断的思考和查资料中，你似乎对EKF的了解又深了一点点。此时你
再一次的看了GenerateNavFilterEquations.m，这时你发现，你似乎可以读懂
这个脚本了，这个脚本的语法和python是那么的相似，但其中有几行还是不一样，你通过百度和自己的实际操作，也逐渐的弄明白了。在看脚本的过程中，
你发现，脚本中的状态转移矩阵用的是，以某种运算通过求得了，这时你恍然
大悟，EKF代码中的是和状态转移矩阵相关的。你之前一直在找的，在代码中
压根就不存在，所以你之前一直感觉EKF2和EKF理论不是那么一致。之后，你又找到了EKF2代码中存在的、。对应的找到了EKF理论中的哪些公式。又花了一两周时间，弄明白了这些公式所代表的含义，将EKF2的代码和公式终于对应了起来。但此时你又发现EKF2所用的公式和维基百科中的有一些区别，和你找的其他资料也不大一样，和秦永元老师写的《卡尔曼滤波与组合导航原理》一
书中所写的公式也不一样，不是多一点就是少一点。因为ardupilot的广泛应用，所以你的直觉告诉你，EKF2所用的公式虽然和资料里的都不一样，但事实
证明它是可用的，那它这么用公式也是可以的。为了加深你的理解，将EKF2所使用的公式，对照着代码写了一遍。此时你已经明白了EKF2所使用的公式有哪些，状态向量、状态转移矩阵、观测矩阵和协方差矩阵都是什么。你已经知道
了在有新的传感器后，如何添加到EKF2中，使其提高系统对自身姿态、位置或速度的估计。进行怎样的测试，来证明你的添加量是能用的。但你此时依然不
清楚EKF2所用的公式为什么是那样，换个公式行不行，EKF的哪些参数影响了
它的结果，影响有多大？是如何影响的？
第三阶段、知其所以然
在经历了一、二阶段之后，你对EKF和其在ardupilot中的实现（EKF2）
有了一定的了解，但第二阶段完成后，你还是有许多的疑问。要解决这些疑问
你必须要对EKF有更深层次的了解，知道EKF哪些公式是怎么推导来的。这需
要较深的数学基础。第三阶段注定是个硬骨头，完成它需要很多理论知识，包
括数学知识、EKF相关的知识、导航相关的知识、传感器相关的知识。但你了
解了这些知识，并用这些知识解答了EKF2为什么要选取那几个公式，那几个公式是不是最优选择的问题。在解决了这些问题后，你可以针对多旋翼用C/C++
实现一个支持IMU、GPS和磁罗盘的位姿解算算法。如果这些事情你都做完了，恭喜你，你已经成为EKF方面的大牛了。
三、一睹EKF2芳容
预测过程
首先我们来确定EKF算法预测过程两个方程（预测状态估计方程和预测协
方差估计方程）中的函数和变量。也就是状态向量Xk、函数f(xk-1,uk-1,wk-1)、状态转移矩阵F、协方差矩阵P、过程噪声协方差矩阵Q和不知名的矩阵L。其中P和Q的初值是根据相关传感器和被估计量的特性定出来的，具体为什么
定的是代码中写的那些值，我目前也不清楚。
定义状态向量：
Xk总共有24个状态，分别是以旋转矢量表示的三轴角度误差、三轴速度（北东地坐标系）、三轴位置（北东地坐标系，以滤波器开始运行的点为坐标
原点）、陀螺仪三轴偏差、陀螺仪三轴比例因子、加速度计Z轴偏差、三轴地
磁场（地磁场在北东地坐标系下的三轴分量）、三轴磁偏量（磁罗盘和机体坐
标系没有对齐而产生的偏差，注意body_magfield表示的并不是磁罗盘测试出
的三轴磁分量！！！）、两轴风速（北东地坐标系下的北和东）。四元数并不
在状态向量中。
//本代码段在AP_NavEKF2_core.h中
Vector28 statesArray;
struct state_elements {
Vector3f angErr; // 0..2
Vector3f velocity; // 3..5
Vector3f position; // 6..8
Vector3f gyro_bias; // 9..11
Vector3f gyro_scale; // 12..14
float accel_zbias; // 15
Vector3f earth_magfield; // 16..18
Vector3f body_magfield; // 19..21
Vector2f wind_vel; // 22..23
Quaternion quat; // 24..27
} &stateStruct;
定义协方差矩阵：在void NavEKF2_core::CovarianceInit()函数中。
定义过程噪声协方差矩阵，其中三轴角度误差、三轴速度（北东地坐标系）、三轴位置（北东地坐标系）的噪声是非加性的，在SG中，所以24维向
量processNoise的前9个元素是0。在函数
NavEKF2_core::CovariancePrediction()中
下面的一段代码出自GenerateNavFilterEquations.m，它的作用是定义了，进而推导出了状态转移矩阵、和。EKF2所用到的核心公式，全部出自这个matlab脚本，各个矩阵的组成也是matlab算出来的。
首先这段脚本先定义了一大堆符号变量（通过syms dax day daz 'real'
这样的形式），注意这个符号变量和C/C++语言中的变量是有区别的，符号变
量就是个符号（这么说好像有点绕），不用给它赋值，这个和高中数学里面解
方程一样，比如，和就是符号变量，当和等于不同值的时候得到相应的值。这
些符号变量组成了EKF公式中的函数和矩阵，在EKF2中这些符号变量，变成了变量，给相应的变量赋值后，经过运算得出了卡尔曼增益和最后的状态估计向量。matlab的结果是可以包含符号变量，这个功能实在是太好用了。也许是一
直使用C++的原因，刚看这个脚本的时候，就符号变量这一点，想了好长时间
才想明白。
接下来我先说下状态向量，他的组成如下：
errRotVec = [rotErrX;rotErrY;rotErrZ];
stateVector =
[errRotVec;vn;ve;vd;pn;pe;pd;dax_b;day_b;daz_b;dax_s;day_s;daz_s;dvz_ b;magN;magE;magD;magX;magY;magZ;vwn;vwe]
状态向量中的每一个成员就相当于一个自变量，你可以把他们看成是。
然后是新的状态向量，其实就是由状态向量经过一些运算推导出来的。学
术一点的说法就是通过一定的规则映射成。说到这里如果熟悉函数定义的你，
应该已经明白了，就是。如果分开写的，（脚本定义中把角度误差、速度、位
置分别写成了3维向量的格式）中的成员就是。
ardupilot(EKF)扩展卡尔曼滤波
ardupilot(EKF)扩展卡尔曼滤波 一、初识卡尔曼滤波器 为了描述方便我从网上找了一张卡尔曼滤波器的5大公式的图片。篇幅所限，下图所示的是多维卡尔曼滤波器（因为EKF2是多维扩展卡尔曼滤波器，所以我们从多维说起），为了跟好的理解卡尔曼滤波器可以百度一下，从一维开始。 这5个公式之外还有一个观测模型，根据你实际的观测量来确定，它的主 要作用是根据实际情况来求观测矩阵H。 因为卡尔曼滤波器是线性滤波器，状态转移矩阵A和观测矩阵H是确定的。在维基百科上状态转移矩阵用F表示。在ardupilot EKF2算法中，状态转移矩阵也是用F表示的。下面是维基百科给出的线性卡尔曼滤波器的相关公式。
上述更新（后验）估计协方差的公式对任何增益K k都有效，有时称为约瑟夫形式。为了获得最佳卡尔曼增益，该公式进一步简化为P k|k=(I-K k H k)P k|k-1，它在哪种形式下应用最广泛。但是，必须记住它仅对最小化残差误差的最佳增益有效。 为了使用卡尔曼滤波器来估计仅给出一系列噪声观测过程的内部状态，必须根据卡尔曼滤波器的框架对过程进行建模，这意味着指定一下矩阵：
只要记住一点就行了，卡尔曼滤波器的作用就是输入一些包含噪声的数据，得到一些比较接近真是情况的数据。比如无人机所使用的陀螺仪和加速度计的 读值，他们的读值都是包含噪声的，比如明明真实的角速度是俯仰2°/s,陀螺 仪的读值却是2.5°/s。通过扩展卡尔曼之后的角速度值会变得更加接近2o/s 的真实值，有可能是2.1o/s。 二、扩展卡尔曼滤波器 因为卡尔曼滤波器针对的是线性系统，状态转移模型（说的白话一点就是知道上一时刻被估计量的值，通过状态转移模型的公式可以推算出当前时刻被 估计量的值）和观测模型。注：有的资料显示状态模型中有,有的没有，目前 我也不清楚是为什么，有可能和被估计的对象有关。但看多了你就会发现不管 网上给的公式有怎样的不同，但总体的流程是一样的，都是这5大步骤。我个 人觉得维基百科给的公式较为标准。 因为扩展卡尔曼滤波器（EKF，Extended Kalman filter）的使用场景为非线性系统。所以上面两公式改写为下面所示的样子，我个人的理解是，因为是 非线性系统，所以没有固定的状态转移矩阵和观测矩阵。到这儿为止卡尔曼滤 波器到扩展卡尔曼滤波器的过度就完成了（多说一句，因为传感器的数据采样 是有时间间隔的，算法的运行也是有间隔的，所以本文提到的KF和EKF都是离散型的）。下面是扩展卡尔曼滤波器的相关公式。
扩展卡尔曼滤波matlab程序
文件一 % THIS PROGRAM IS FOR IMPLEMENTATION OF DISCRETE TIME PROCESS EXTENDED KALMAN FILTER % FOR GAUSSIAN AND LINEAR STOCHASTIC DIFFERENCE EQUATION. % By (R.C.R.C.R),SPLABS,MPL. % (17 JULY 2005). % Help by Aarthi Nadarajan is acknowledged. % (drawback of EKF is when nonlinearity is high, we can extend the % approximation taking additional terms in Taylor's series). clc; close all; clear all; Xint_v = [1; 0; 0; 0; 0]; wk = [1 0 0 0 0]; vk = [1 0 0 0 0]; for ii = 1:1:length(Xint_v) Ap(ii) = Xint_v(ii)*2; W(ii) = 0; H(ii) = ‐sin(Xint_v(ii)); V(ii) = 0; Wk(ii) = 0; end Uk = randn(1,200); Qu = cov(Uk); Vk = randn(1,200); Qv = cov(Vk); C = [1 0 0 0 0]; n = 100; [YY XX] = EKLMNFTR1(Ap,Xint_v,Uk,Qu,Vk,Qv,C,n,Wk,W,V); for it = 1:1:length(XX) MSE(it) = YY(it) ‐ XX(it); end tt = 1:1:length(XX); figure(1); subplot(211); plot(XX); title('ORIGINAL SIGNAL'); subplot(212); plot(YY); title('ESTIMATED SIGNAL'); figure(2); plot(tt,XX,tt,YY); title('Combined plot'); legend('original','estimated'); figure(3); plot(MSE.^2); title('Mean square error'); 子文件：：function [YY,XX] = EKLMNFTR1(Ap,Xint_v,Uk,Qu,Vk,Qv,C,n,Wk,W,V); Ap(2,:) = 0; for ii = 1:1:length(Ap)‐1 Ap(ii+1,ii) = 1;
基于FPGA的卡尔曼滤波器的设计
基于FPGA的卡尔曼滤波器的设计 时间：2010-04-12 12:52:33 来源：电子科技作者：米月琴，黄军荣西安电子科技大学摘要：针对电路设计中经常碰到数据的噪声干扰现象，提出了一种Kalman滤波的FPGA实现方法。该方法采用了TI公司的高精度模数转换器ADSl25l以及Altera公司的EPlCl2，首先用卡尔曼滤波算法 设计了一个滤波器，然后将该滤波器分解成简单的加、减、乘、除运算。通过基于FPGA平台的硬件与 软件的合理设计，成功地实现了数据噪声的滤除设计，并通过实践仿真计算，验证了所实现滤波的有效性。 关键词：卡尔曼；FPGA；最小方差估计 卡尔曼滤波是一个“Optimal Recursive Data Processing Algorithm(最优化自回归数据处 理算法)”，对于解决很大部分的问题，是最优化的，效率最高甚至是最有用的。传统的卡尔曼滤波是 在DSP上实现的。但是DSP成本相对较高，而且指令是串行执行的，不能满足有些要求较高的场合。而FPGA由于其硬件结构决定了它的并行处理方式，无论在速度还是实时性都更胜一筹。文中以基于FPGA 器件和A／D转换器的数据采集系统为硬件平台，进行了卡尔曼滤波算法设计，详述了基于FPGA的卡尔 曼滤波器的设计实现。 1 卡尔曼滤波算法 工程中，为了了解工程对象(滤波中称为系统)的各个物理量(滤波中称为状态)的确切数值，或为了 达到对工程对象进行控制的目的，必须利用测量手段对系统的各个状态进行测量。但是，量测值可能仅 是系统的部分状态或是部分状态的线性组合，且量测值中有随机误差(常称为量测噪声)。最优估计就是 针对上述问题的一种解决方法。它能将仅与部分状态有关的测量进行处理，得出从统计意义上讲误差最 小的更多状态的估值。误差最小的标准常称为估计准则，根据不同的估计准则和估计计算方法，有各种 不同的最优估计，卡尔曼滤波是一种递推线性最小方差估计的最优估计。 系统的状态方程可设定为 式（3）为系统噪声。设设备的量测噪声为Vk，系统得量测方程为
几种卡尔曼滤波算法理论
自适应卡尔曼滤波 卡尔曼滤波发散的原因 如果卡尔曼滤波是稳定的，随着滤波的推进，卡尔曼滤波估计的精度应该越来越高，滤波误差方差阵也应趋于稳定值或有界值。但在实际应用中，随着量测值数目的增加，由于估计误差的均值和估计误差协方差可能越来越大，使滤波逐渐失去准确估计的作用，这种现象称为卡尔曼滤波发散。 引起滤波器发散的主要原因有两点：（1）描述系统动力学特性的数学模型和噪声估计模型不准确，不能直接真实地反映物理过程，使得模型与获得的量测值不匹配而导致滤波发散。这种由于模型建立过于粗糙或失真所引起的发散称为滤波发散。 （2）由于卡尔曼滤波是递推过程，随着滤波步数的增加，舍入误差将逐渐积累。如果计算机字长不够长，这种积累误差很有可能使估计误差方差阵失去非负定性甚至失去对称性，使滤波增益矩阵逐渐失去合适的加权作用而导致发散。这种由于计算舍入误差所引起的发散称为计算发散。 针对上述卡尔曼滤波发散的原因，目前已经出现了几种有效抑制滤波发散的方法，常用的有衰减记忆滤波、限定记忆滤波、扩充状态滤波、有限下界滤波、平方根滤波、和自适应滤波等。这些方法本质上都是以牺牲滤波器的最优性为代价来抑制滤波发散，也就是说，多数都是次优滤波方法。 自适应滤波 在很多实际系统中，系统过程噪声方差矩阵Q和量测误差方差阵R事先是不知道的，有时甚至连状态转移矩阵或量测矩阵H也不能确切建立。如果所建立 的模型与实际模型不符可能回引起滤波发散。自适应滤波就是这样一种具有抑制滤波发散作用的滤波方法。在滤波过程中，自适应滤波一方面利用量测值修正预测值，同时也对未知的或不确切的系统模型参数和噪声统计参数进行估计修正。自适应滤波的方法很多，包括贝叶斯法、极大似然法、相关法与协方差匹配法，其中最基本也是最重要的是相关法，而相关法可分为输出相关法和新息相关法。 在这里只讨论系统模型参数已知，而噪声统计参数Q和R未知情况下的自适应滤波。由于Q和R等参数最终是通过增益矩阵K影响滤波值的，因此进行自适应滤波时，也可以不去估计Q和R等参数而直接根据量测数据调整K就可以了。 输出相关法自适应滤波的基本途径就是根据量测数据估计出输出函数序列 {C k}，再由{C讣推算出最佳增益矩阵K,使得增益矩阵K不断地与实际量测数据 {C k} 相适应。
维纳最速下降法滤波器卡尔曼滤波器设计及Matlab仿真
信息融合大作业 ——维纳最速下降法滤波器，卡尔曼滤波器设计及Matlab仿真 1.滤波问题浅谈 估计器或滤波器这一术语通常用来称呼一个系统，设计这样的系统是为了从含有噪声的数据中提取人们感兴趣的，接近规定质量的信息。由于这样一个宽目标，估计理论应用于诸如通信、雷达、声纳、导航、地震学、生物医学工程、 金融工程等众多不同的领域。例如，考虑一个数字通信系统，其基本形式由发
射机、信道和接收机连接组成。发射机的作用是把数字源(例如计算机)产生的0、1符号序列组成的消息信号变换成为适合于信道上传送的波形。而由于符号间干扰和噪声的存在，信道输出端收到的信号是含有噪声的或失真的发送信号。接收机的作用是，操作接收信号并把原消息信号的一个可靠估值传递给系统输出端的某个用户。随着通信系统复杂度的提高，对原消息信号的还原成为通信系统中最为重要的环节，而噪声是接收端需要排除的最主要的干扰，人们也设计出了针对各种不同条件应用的滤波器，其中最速下降算法是一种古老的最优化技术，而卡尔曼滤波器随着应用条件的精简成为了普适性的高效滤波器。2．维纳最速下降算法滤波器 2.1 最速下降算法的基本思想 考虑一个代价函数，它是某个未知向量的连续可微分函数。函数 将的元素映射为实数。这里，我们要寻找一个最优解。使它满足如下条件 (2.1) 这也是无约束最优化的数学表示。 特别适合于自适应滤波的一类无约束最优化算法基于局部迭代下降的算法： 从某一初始猜想出发，产生一系列权向量，使得代价函数在算法的每一次迭代都是下降的，即 其中是权向量的过去值，而是其更新值。 我们希望算法最终收敛到最优值。迭代下降的一种简单形式是最速下降法，该方法是沿最速下降方向连续调整权向量。为方便起见，我们将梯度向量表示为
扩展卡尔曼滤波(EKF)应用于GPS-INS组合导航
clear all; %% 惯性-GPS组合导航模型参数初始化 we = 360/24/60/60*pi/180; %地球自转角速度，弧度/s psi = 10*pi/180; %psi角度/ 弧度 Tge = 0.12; Tgn = 0.10; Tgz = 0.10; %这三个参数的含义详见参考文献 sigma_ge=1; sigma_gn=1; sigma_gz=1; %% 连续空间系统状态方程 % X_dot(t) = A(t)*X(t) + B(t)*W(t) A=[0 we*sin(psi) -we*cos(psi) 1 0 0 1 0 0; -we*sin(psi) 0 0 0 1 0 0 1 0; we*cos(psi) 0 0 0 0 1 0 0 1; 0 0 0 -1/Tge 0 0 0 0 0; 0 0 0 0 -1/Tgn 0 0 0 0; 0 0 0 0 0 -1/Tgz 0 0 0; 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0;]; %状态转移矩阵 B=[0 0 0 sigma_ge*sqrt(2/Tge) 0 0 0 0 0; 0 0 0 0 sigma_gn*sqrt(2/Tgn) 0 0 0 0; 0 0 0 0 0 sigma_gz*sqrt(2/Tgz) 0 0 0;]';%输入控制矩阵%% 转化为离散时间系统状态方程 % X(k+1) = F*X(k) + G*W(k) T = 0.1; [F,G]=c2d(A,B,T);
H=[1 0 0 0 0 0 0 0 0; 0 -sec(psi) 0 0 0 0 0 0 0;];%观测矩阵 %% 卡尔曼滤波器参数初始化 t=0:T:50-T; length=size(t,2); y=zeros(2,length); Q=0.5^2*eye(3); %系统噪声协方差 R=0.25^2*eye(2); %测量噪声协方差 y(1,:)=2*sin(pi*t*0.5); y(2,:)=2*cos(pi*t*0.5); Z=y+sqrt(R)*randn(2,length); %生成的含有噪声的假定观测值，2维X=zeros(9,length); %状态估计值，9维 X(:,1)=[0,0,0,0,0,0,0,0,0]'; %状态估计初始值设定 P=eye(9); %状态估计协方差 %% 卡尔曼滤波算法迭代过程 for n=2:length X(:,n)=F*X(:,n-1); P=F*P*F'+ G*Q*G'; Kg=P*H'/(H*P*H'+R); X(:,n)=X(:,n)+Kg*(Z(:,n)-H*X(:,n)); P=(eye(9,9)-Kg*H)*P; end %% 绘图代码 figure(1) plot(y(1,:)) hold on; plot(y(2,:)) hold off; title('理想的观测量'); figure(2)
直流电机运行状态的卡尔曼滤波估计器设计.doc
二 〇 一 五 年 六 月 题 目：直流电机运行状态的卡尔曼滤波估计器设计 学生姓名：张傲 学 院：电力学院 系 别：电力系 专 业：风能与动力工程 班 级：风能11-1 指导教师：董朝轶 教授
摘要 卡尔曼滤波是一个迭代自回归算法，对于连续运动状态用中的大部分问题它都能够给出最优的预测。它已经广泛应用了近半个世纪，例如数据的融合，机械的导航乃至军用雷达的导航等等。卡尔曼滤波一般用于动态数据的处理，是从混沌的信号中提取有用信号消除误差的参数估计法。卡尔曼滤波是依据上一个估计数值和当下的检测数据运用递推估计算出当前的估计值。通过状态方程运用递推的方法进行估计,可以建立物体运动的模型。本文采用的工程设计对运行状态下的直流电机进行参数的计算和校验。而且直流电机的调节性能非常好只需要加上电阻调压就可以了，而且启动曲线非常好，启动的转矩大适合高精度的控制。而交流电机调速需要变频，控制相对复杂一些，而对于设计无论是哪种电机都不影响结果，所以本实验采用直流电机。简单来说卡尔曼滤波就是对被观测量进行一个物理的建模，目的是用‘道理’来约束观测结果，减少噪声的影响。因此卡尔曼滤波是根据一个事物的当前状态预测它的下一个状态的过程。 此设计主要是通过对直流电机的数学模型利用MATLAB来设计卡尔曼滤波估计，进行仿真编程建模，进而对系统进行评估，并且分析估计误差。 关键词：卡尔曼滤波器；直流电机；MATLAB
Abstract Kalman filter is an iterative autoregression algorithm for continuous motion of most of the problems with it are able to give the best prediction. And it has been widely used for nearly half a century, such as the integration of data, as well as military machinery of navigation radar navigation, and so on. Kalman filter is generally used to process dynamic data, extract useful signal parameter estimation method to eliminate errors from the chaotic signal. Kalman filter is based on an estimate on the value and the current detection data is calculated using recursive estimation current estimates. By using recursive state equation method to estimate the movement of objects can be modeled. The paper describes the engineering design of the DC motor running state parameter calculation and verification. The DC motor performance and adjust very well simply by adding resistance regulator on it, and start curve is very good, start torque for precision control. The required frequency AC motor speed control is relatively complicated, and for the design of either the motor does not affect the outcome.In order to facilitate learning, so wo use the DC motor. Simply the Kalman filter is to be observables conduct a physical modeling; the purpose is to use 'sense' to restrict the observations to reduce the influence of noise. Therefore, the Kalman filter is based on the current state of things predict its next state of the process. This design is mainly through the DC motor mathematical model using MATLAB to design the Kalman filter estimation, simulation modeling program, and then to evaluate the system and analyze the estimation error. Keywords：Kalman filter; DC;MATLAB
卡尔曼滤波器介绍 --- 最容易理解
10.6 卡尔曼滤波器简介 本节讨论如何从带噪声的测量数据把有用信号提取出来的问题。通常，信号的频谱处于有限的频率范围内，而噪声的频谱则散布在很广的频率范围内。如前所述，为了消除噪声，可以把 FIR滤波器或IIR滤波器设计成合适的频带滤波器，进行频域滤波。但在许多应用场合，需要进行时域滤波，从带噪声的信号中提取有用信号。虽然这样的过程其实也算是对信号的滤波，但所依据的理论,即针对随机信号的估计理论,是自成体系的。人们对随机信号干扰下的有用信号不能“确知”，只能“估计”。为了“估计”，要事先确定某种准则以评定估计的好坏程度。最小均方误差是一种常用的比较简单的经典准则。典型的线性估计器是离散时间维纳滤波器与卡尔曼滤波器。 对于平稳时间序列的最小均方误差估计的第一个明确解是维纳在1942年2月首先给出的。当时美国的一个战争研究团体发表了一个秘密文件，其中就包括维纳关于滤波问题的研究工作。这项研究是用于防空火力控制系统的。维纳滤波器是基于最小均方误差准则的估计器。为了寻求维纳滤波器的冲激响应，需要求解著名的维纳-霍夫方程。这种滤波理论所追求的是使均方误差最小的系统最佳冲激响应的明确表达式。这与卡尔曼滤波（Kalman filtering）是很不相同的。卡尔曼滤波所追求的则是使均方误差最小的递推算法。 在维纳进行滤波理论研究并导出维纳-霍夫方程的十年以前，在1931年，维纳和霍夫在数学上就已经得到了这个方程的解。 对于维纳-霍夫方程的研究，20世纪五十年代涌现了大量文章，特别是将维纳滤波推广到非平稳过程的文章甚多，但实用结果却很少。这时正处于卡尔曼滤波问世的前夜。 维纳滤波的困难问题，首先在上世纪五十年代中期确定卫星轨道的问题上遇到了。1958年斯韦尔林（Swerling）首先提出了处理这个问题的递推算法，并且立刻被承认和应用。1960年卡尔曼进行了比斯韦尔林更有意义的工作。他严格地把状态变量的概念引入到最小均方误差估计中来，建立了卡尔曼滤波理论。空间时代的到来推动了这种滤波理论的发展。 维纳滤波与卡尔曼滤波所研究的都是基于最小均方误差准则的估计问题。 维纳滤波理论的不足之处是明显的。在运用的过程中，它必须把用到的全部数据存储起来，而且每一时刻都要通过对这些数据的运算才能得到所需要的各种量的估值。按照这种滤波方法设置的专用计算机的存储量与计算量必然很大，很难进行实时处理。虽经许多科技工作者的努力，在解决非平稳过程的滤波问题时，给出能用的方法为数甚少。到五十年代中期，随着空间技术的发展，这种方法越来越不能满足实际应用的需要，面临了新的挑战。尽管如此，维纳滤波理论在滤波理论中的开拓工作是不容置疑的，维纳在方法论上的创见，仍然影响着后人。 五十年代中期，空间技术飞速发展，要求对卫星轨道进行精确的测量。为此，人们将滤波问题以微分方程表示，提出了一系列适应空间技术应用的精练算法。1960年
扩展卡尔曼滤波器(EKF)：一个面向初学者的交互式教程-翻译
扩展卡尔曼滤波器教程 在使用OpenPilot和Pixhawk飞控时，经常遇到扩展卡尔曼滤波(EKF)。从不同的网页和参考论文中搜索这个词，其中大部分都太深奥了。所以我决定创建自己学习教程。本教程从一些简单的例子和标准(线性)卡尔曼滤波器，通过对实际例子来理解卡尔曼滤波器。 Part 1: 一个简单的例子 想象一个飞机准备降落时，尽管我们可能会担心许多事情，像空速、燃料、等等，当然最明显是关注飞机的高度(海拔高度)。通过简单的近似，我们可以认为当前高度是之前的高度失去了一小部分。例如，当每次我们观察飞行高度时，认为飞机失去了2%的高度，那么它的当前高度是上一时刻高度的98%: altitude current_time=0.98*altitude previous_time 工程上对上面的公式，使用“递归”这个术语进行描述。通过递归前一时刻的值，不断计算当前值。最终我们递归到初始的“基本情况”，比如一个已知的高度。 试着移动上面的滑块，看看飞机针对不同百分比的高度变化。 Part 2:处理噪声 当然, 实际从传感器比如GPS或气压计获得测量高度时，传感器的数据或多或少有所偏差。如果传感器的偏移量为常数，我们可以简单地添加或减去这偏移量来确定我们的高度。不过通常情况下，传感器的偏移量是一个时变量，使得我们所观测到的传感器数据相当于实际高度加上噪声: observed_altitude current_time=altitude current_time+noise current_time 试着移动上面的滑块看到噪声对观察到的高度的影响。噪音被表示为可观测的海拔范围的百分比。
卡尔曼滤波器
卡尔曼滤波器 来这里几个月，发现有些问题很多人都很感兴趣。所以在这里希望能尽自己能力跟大家讨论一些力所能及的算法。现在先讨论一下卡尔曼滤波器，如果时间和能力允许，我还希望能够写写其他的算法，例如遗传算法，傅立叶变换，数字滤波，神经网络，图像处理等等。 因为这里不能写复杂的数学公式，所以也只能形象的描述。希望如果哪位是这方面的专家，欢迎讨论更正。 卡尔曼滤波器– Kalman Filter 1．什么是卡尔曼滤波器 （What is the Kalman Filter?） 在学习卡尔曼滤波器之前，首先看看为什么叫“卡尔曼”。跟其他著名的理论（例如傅立叶变换，泰勒级数等等）一样，卡尔曼也是一个人的名字，而跟他们不同的是，他是个现代人！ 卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。如果对这编论文有兴趣，可以到这里的地址下载：https://www.360docs.net/doc/1e8082423.html,/~welch/media/pdf/Kalman1960.pdf。 简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。 2．卡尔曼滤波器的介绍 （Introduction to the Kalman Filter） 为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。 在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。
卡尔曼滤波器综述
卡尔曼滤波器综述 瞿伟军 G10074 1、卡尔曼滤波的起源 1960年，匈牙利数学家卡尔曼发表了一篇关于离散数据线性滤波递推算法的论文，这意味着卡尔曼滤波的诞生。斯坦利.施密特(Stanley Schmidt)首次实现了卡尔曼滤波器，卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑使用了这种滤波器。关于这种滤波器的论文由Swerling (1958)、Kalman (1960)与 Kalman and Bucy (1961)发表。 2、卡尔曼滤波的发展 卡尔曼滤波是一种有着相当广泛应用的滤波方法，但它既需要假定系统是线性的，又需要认为系统中的各个噪声与状态变量均呈高斯分布，而这两条并不总是确切的假设限制了卡尔曼滤波器在现实生活中的应用。扩展卡尔曼滤波器（EKF）极大地拓宽了卡尔曼滤波的适用范围。EKF的基本思路是，假定卡尔曼滤滤对当前系统状态估计值非常接近于其真实值，于是将非线性函数在当前状态估计值处进行台劳展开并实现线性化。另一种非线性卡尔曼滤波叫线性化卡尔曼滤波。它与EKF的主要区别是前者将非线函数在滤波器对当前系统状态的最优估计值处线性化，而后者因为预先知道非线性系统的实际运行状态大致按照所要求、希望的轨迹变化，所以这些非线性化函数在实际状态处的值可以表达为在希望的轨迹处的台劳展开式，从而完成线性化。 不敏卡尔曼滤波器（UKF）是针对非线性系统的一种改进型卡尔曼滤波器。UKF处理非线性系统的基本思路在于不敏变换，而不敏变换从根本上讲是一种描述高斯随机变量在非线性化变换后的概率分布情况的方法。不敏卡尔曼滤波认为，与其将一个非线性化变换线性化、近似化，还不如将高斯随机变量经非线性变换后的概率分布情况用高斯分布来近似那样简单，因而不敏卡尔曼滤波算法没
扩展卡尔曼滤波器(EKF)进行信号处理及信号参数估计
% 扩展卡尔曼滤波器估计单相电压幅值、相位、频率参数（含直流）function test2_EKF close all; clc; tic; %计时 %模型：y=A0+A1*cos(omega*t+phy1) %离散化：y(k)=A0(k)+A1(k)*cos(omega(k)*k*Ts+phy1(k)) %状态变量：x1(k)=A0(k),x2(k)=omega(k),x3(k)=A1(k)*cos(omega(k)*k*Ts+phy1(k) ),x4(k)=A1(k)*sin(omega(k)*k*Ts+phy1(k)) %下一时刻状态变量为(假设状态不突变)：A0(k+1)=A0(k),A1(k+1)=A1(k),omega(k+1)=omega(k),phy1(k+1)=phy1 (k); %则对应状态为：x1(k+1)=x1(k),x2(k+1)=x2(k),x3(k+1)=x3(k)*cos(x2(k)*Ts)- x4(k)*sin(x(2)*Ts),x4(k+1)=x3(k)*sin(x2(k)*Ts)+x4(k)*cos(x(2)*Ts); %状态空间描述：X(k+1)=f(X(k))+W(k);y(k)=H*X(k)+v(k) %f(X(k))=[x1(k);x2(k);x3(k)*cos(x2(k)*Ts)- x4(k)*sin(x(2)*Ts);x3(k)*sin(x2(k)*Ts)+x4(k)*cos(x(2)*Ts)] %偏导(只求了三个)：f`(X(k))=[1,0,0;0,1,0;0,-x3(k)*Ts*sin(x2(k)*Ts)-x4(k)*Ts*cos(x2(k)*Ts),cos(x2(k)*Ts);0,x3(k)*Ts*cos(x2(k)*Ts)- x4(k)*Ts*sin(x2(k)*Ts),sin(x2(k)*Ts)]
拓展卡尔曼滤波
南京航空航天大学 随机信号小论文题目扩展卡尔曼滤波 学生姓名梅晟 学号SX1504059 学院电子信息工程学院 专业通信与信息系统
扩展卡尔曼滤波 一、引言 20世纪60年代，在航空航天工程突飞猛进而电子计算机又方兴未艾之时，卡尔曼发表了论文《A New Approach to Linear Filtering and Prediction Problems》（一种关于线性滤波与预测问题的新方法），这让卡尔曼滤波成为了时域内有效的滤波方法，从此各种基于卡尔曼滤波的方法横空出世，在目标跟踪、故障诊断、计量经济学、惯导系统等方面得到了长足的发展。 二、卡尔曼滤波器 卡尔曼滤波是一种高效率的递归滤波器(自回归滤波器), 它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。卡尔曼滤波的一个典型实例是从一组有限的，包含噪声的，对物体位置的观察序列（可能有偏差）预测出物体的位置的坐标及速度。 卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑便使用了这种滤波器。目前,卡尔曼滤波已经有很多不同的实现。卡尔曼最初提出的形式现在一般称为简单卡尔曼滤波器。除此以外，还有施密特扩展滤波器、信息滤波器以及很多Bierman, Thornton 开发的平方根滤波器的变种。也许最常见的卡尔曼滤波器是锁相环，它在收音机、计算机和几乎任何视频或通讯设备中广泛存在。 三、扩展卡尔曼滤波器 3.1 被估计的过程信号 卡尔曼最初提出的滤波理论只适用于线性系统,Bucy，Sunahara等人提出并研究了扩展卡尔曼滤波(Extended Kalman Filter，简称EKF)，将卡尔曼滤波理论进一步应用到非线性领域。EKF的基本思想是将非线性系统线性化，然后进行卡尔曼滤波，因此EKF是一种次优滤波。 同泰勒级数类似，面对非线性关系时，我们可以通过求过程和量测方程的偏导来线性化并计算当前估计。假设过程具有状态向量x∈?n，其状态方程为非线性随机差分方程的形式。 x k=f x k?1,u k?1,w k?1(1.1) 观测变量z∈?m为： z k=?(x k,v k)(1.2) 随机变量w k和v k代表过程激励噪声和观测噪声。它们为相互独立，服从正态分布的白色噪声：
matlab对卡尔曼滤波的仿真实现
MATLAB 对卡尔曼滤波器的仿真实现 刘丹，朱毅，刘冰 武汉理工大学信息工程学院，武汉（430070） E-mail ：liudan_ina@https://www.360docs.net/doc/1e8082423.html, 摘 要：本文以卡尔曼滤波器原理为理论基础，用MATLAB 进行卡尔曼滤波器仿真、对比卡尔曼滤波器的预测效果，对影响滤波其效果的各方面原因进行讨论和比较，按照理论模型进行仿真编程，清晰地表述了编程过程。 关键词：数字信号处理；卡尔曼滤波器；MATLAB ；仿真过程 中图分类号: TN912.3 1. 引言 随着信息时代和数字世界的到来，数字信号处理已成为当今一门极其重要的学科和技术领域。数字信号处理已在通信、语音、图像、自动控制、雷达、军事、航空航天、医疗和家用电器等众多领域得到了广泛的应用。在数字信号处理中,数字滤波占有极其重要的地位,目前对数字滤波器的设计有多种方法,其中著名的MATLAB 软件包在多个研究领域都有着广泛的应用,它的频谱分析[1]和滤波器的分析设计功能很强,从而使数字信号处理变得十分简单、直观。本文分析了数字滤波器的设计方法,举出了基于MATLAB 软件的信号处理工具在数字滤波器设计中的应用。 2. 卡尔曼滤波基本原理 卡尔曼滤波过程实际上是获取维纳解的递推运算过程[2]。从维纳解导出的卡尔曼滤波器实际上是卡尔曼滤波过程结束后达到稳态的情况，这时Kalman Filtering 的结果与Wiener Solution 是相同的[3]。具体推导如下： )()1|1(?)|(?n Gy n n x f n n x +??= )|(?)()(n n x n x n e ?= 已知由此求c a cG a f F G n e E n ,)1(( ..min )]([)(2?=??→?==ε 由 f G f G ,0??????????=??εε ⑴ )]1|1(?)()[()1|1(?)|(????+??=n n x ac n y n G n n x a n n x 可以是时变的，非平稳的随机信号 ⑵ Q n a n P +?=)1()(2 ε均为正数。 ⑶ ) () ()(2n P C R n CP n G += ⑷ )()](1[)()(n P n CG n G C P n ??== ε )(n G 是个随时间变化的量，每次输入输出，)(n G 就调整一次，并逐渐逼近Kalman Filter 的增益G ，而)1()(? 卡尔曼滤波器介绍 Greg Welch1and Gary Bishop2 TR95-041 Department of Computer Science University of North Carolina at Chapel Hill3 Chapel Hill,NC27599-3175 翻译：姚旭晨 更新日期:2006年7月24日，星期一 中文版更新日期：2007年1月8日，星期一 摘要 1960年，卡尔曼发表了他著名的用递归方法解决离散数据线性滤波问题的论文。从那以后，得益于数字计算技术的进步，卡尔曼滤波器已成为推广研究和应用的主题，尤其是在自主或协助导航领域。 卡尔曼滤波器由一系列递归数学公式描述。它们提供了一种高效可计算的方法来估计过程的状态，并使估计均方误差最小。卡尔曼滤波器应用广泛且功能强大：它可以估计信号的过去和当前状态，甚至能估计将来的状态，即使并不知道模型的确切性质。 这篇文章介绍了离散卡尔曼理论和实用方法，包括卡尔曼滤波器及其衍生：扩展卡尔曼滤波器的描述和讨论，并给出了一个相对简单的带图实例。 1welch@https://www.360docs.net/doc/1e8082423.html,,https://www.360docs.net/doc/1e8082423.html,/?welch 2gb@https://www.360docs.net/doc/1e8082423.html,,https://www.360docs.net/doc/1e8082423.html,/?gb 3北卡罗来纳大学教堂山分校，译者注。 1 Welch&Bishop,卡尔曼滤波器介绍2 1离散卡尔曼滤波器 1960年，卡尔曼发表了他著名的用递归方法解决离散数据线性滤波问题的论文[Kalman60]。从那以后，得益于数字计算技术的进步，卡尔曼滤波器已成为推广研究和应用的主题，尤其是在自主或协助导航领域。[Maybeck79]的第一章给出了一个非常“友好”的介绍，更全面的讨论可以参考[Sorenson70]，后者还包含了一些非常有趣的历史故事。更广泛的参考包括[Gelb74,Grewal93,Maybeck79,Lewis86,Brown92,Jacobs93]。 被估计的过程信号 卡尔曼滤波器用于估计离散时间过程的状态变量x∈ n。这个离散时间过程由以下离散随机差分方程描述： x k=Ax k?1+Bu k?1+w k?1,(1.1)定义观测变量z∈ m，得到量测方程： z k=Hx k+v k.(1.2)随机信号w k和v k分别表示过程激励噪声1和观测噪声。假设它们为相互独立，正态分布的白色噪声： p(w)～N(0,Q),(1.3) p(v)～N(0,R).(1.4)实际系统中，过程激励噪声协方差矩阵Q和观测噪声协方差矩阵R可能会随每次迭代计算而变化。但在这儿我们假设它们是常数。 当控制函数u k?1或过程激励噪声w k?1为零时，差分方程1.1中的n×n 阶增益矩阵A将上一时刻k?1的状态线性映射到当前时刻k的状态。实际中A可能随时间变化，但在这儿假设为常数。n×l阶矩阵B代表可选的控制输入u∈ l的增益。量测方程1.2中的m×n阶矩阵H表示状态变量x k 对测量变量z k的增益。实际中H可能随时间变化，但在这儿假设为常数。滤波器的计算原型 定义?x?k∈ n（?代表先验，?代表估计）为在已知第k步以前状态情况下第k步的先验状态估计。定义?x k∈ n为已知测量变量z k时第k步的后验状态估计。由此定义先验估计误差和后验估计误差： ≡x k??x?k, e? k e k≡x k??x k 1原文为process noise，本该翻译作过程噪声，由时间序列信号模型的观点，平稳随机序列可以看成是由典型噪声源激励线性系统产生，故译作过程激励噪声。 UNC-Chapel Hill,TR95-041,July24,2006 第三章扩展卡尔曼滤波EKF 3.1 扩展Kalman滤波原理 Kalman滤波能够在线性高斯模型的条件下，可以对目标的状态做出最优的估计，得到较好的跟踪效果。对非线性滤波问题常用的处理方法是利用线性化技巧将其转化为一个近似的线性滤波问题。因此，可以利用非线性函数的局部性特性，将非线性模型局部化，再利用Kalman滤波算法完成滤波跟踪。扩展Kalman 滤波就是基于这样的思想，将系统的非线性函数做一阶Taylor展开，得到线性化的系统方程从而完成对目标的滤波估计等处理。 非线性系统离散动态方程可以表示为 (k1)f[k,X(k)]G(k)W(k) X+=+（3-1-1） Z=+（3-1-2） (k)h[k,X(k)]V(k) 这里为了便于数学处理，假定没有控制量的输入，并假定过程噪声是均值为零的高斯白噪声，且噪声分布矩阵(k) V也是加性均 G是已知的。其中，观测噪声(k) 值为零的高斯白噪声。假定过程噪声和观测噪声序列是彼此独立的，并且有初始状态估计?(0|0) X和协方差矩阵(0|0) P。和线性系统的情况一样，我们可以得到扩展Kalman滤波算法如下 ?? X+=（3-1-3） (k|k1)f(X(k|k)) ?（3-1-4） P+=Φ+Φ+++ (k1|k)(k1|k)P(k|k)(k1|k)Q(k1) 晻（3-1-5） 1 (k1)P(k1|)H(k1)[H(k1)P(k1|k)H(k1)R(k1)] +=+++++++ K k- ??? (K1|k1)X(K1|k)K(k1)[Z(k1)h(X(K1|k))] X++=++++-+（3-1-6）P+=-+++（3-1-7） (k1)[I K(k1)H(k1)]P(k1|k) 第27卷第36期中国电机工程学报V ol.27 No.36 Dec. 2007 2007年12月Proceedings of the CSEE ?2007 Chin.Soc.for Elec.Eng. 文章编号：0258-8013 (2007) 36-0036-05 中图分类号：TM 351 文献标识码：A 学科分类号：470?40 基于扩展卡尔曼滤波器的永磁同步电机 转速和磁链观测器 张 猛，肖 曦，李永东 (电力系统及发电设备控制和仿真国家重点实验室(清华大学电机系)，北京市海淀区 100084) Speed and Flux Linkage Observer for Permanent Magnet Synchronous Motor Based on EKF ZHANG Meng, XIAO Xi , LI Yong-dong (State Key Lab of Control and Simulation of Power Systems and Generation Equipments (Dept. of Electrical Engineering, Tsinghua University), Haidian District, Beijing 100084, China) ABSTRACT: To eliminate the mechanical sensors of permanent magnet synchronous motor (PMSM) drive and get the stator flux linkage used in direct torque control (DTC), an extended Kalman filter (EKF) is established. The stator flux linkage on the fixed α-β coordinate, rotor speed and position are chosen as state variables. The input and output of the EKF are stator voltages and currents. The stator flux linkage and rotor speed are observed by EKF. DTC using space vector modulation (SVM) is applied to the system in order to reduce the torque ripples and keep constant switching frequency. The experimental test is carried out to verify the efficiency and robustness of the proposed sensor-less DTC system with speed and flux linkage observer. KEY WORDS: permanent magnet synchronous motor; extended Kalman filter; direct torque control; sensor-less; space vector modulation 摘要：为了取消永磁同步电机控制中的机械传感器，获得直接转矩控制中需要的电机磁链信息，设计了一种基于扩展卡尔曼滤波器的永磁同步电机转速和磁链估算方法。选取定子固定坐标系下定子磁链、电机转速和转子位置为状态变量，电压和电流作为输入、输出量，建立估算定子磁链、电机转速和转子位置的EKF滤波器系统。采用空间矢量调制的直接转矩控制策略，有效减小了直接转矩控制方法的转矩脉动，并保持了功率器件恒定的开关频率。实验结果表明EKF 准确地观测了电机转速和磁链，所构建的无速度传感器DTC 控制系统具有良好的转速和转矩控制性能。 基金项目：国家自然科学基金项目(50607010)。 Project Supported by National Natural Science Foundation of China (50607010)．关键词：永磁同步电机；扩展卡尔曼滤波器；直接转矩控制；无速度传感器；空间矢量调制 0 引言 永磁同步电机直接转矩控制具有快速的转矩响应和良好的动态性能，吸引了很多学者进行相关研究，并取得了一定的研究成果[1-5]。在直接转矩控制当中，需要机械传感器提供电机的转速信息，机械传感器使系统复杂性增加，鲁棒性降低。磁链观测的准确性直接影响到转矩的控制性能。在无机械传感器的情况下，如何获得准确的速度和磁链信息是永磁同步电机直接转矩控制研究方面的热点问题。 传统的永磁同步电机直接转矩控制中磁链观测大多采用纯积分方法。初值的敏感性和直流漂移是纯积分方法的主要缺点。三种改进型积分器已经应用到异步电机磁链估算中[6]，其中的第二种方法被用于永磁同步电机磁链的估算[7]，但是在全速范围内只能够获得准确的相位信息。全阶观测器[8]和非线性反馈正交磁链补偿观测器[9]也被应用到永磁同步电机磁链观测中，但是此两种方法需要利用传感器或其他算法获得电机转速。 电机的转速可以通过对定子磁链位置变化或通过采用定子磁链和转矩角获得转子位置变化计算得到[10-11]，但是需要在输出加一个低通滤波器以获得平滑的估算速度，滤波器的延时容易造成系统的不稳定。 基于扩展卡尔曼滤波器对非线性系统优异的状态估算能力，本文提出一种能同时观测速度和磁链的方法，实现了永磁同步电机的无速度传感器直接卡尔曼滤波器介绍
扩展卡尔曼滤波EKF
基于扩展卡尔曼滤波器的永磁同步电机转速和磁链观测器.kdh