ukf(无迹卡尔曼滤波)算法的matlab程序. function [x,P]=ukf(fstate,x,P,hmeas,z,Q,R)
% UKF Unscented Kalman Filter for nonlinear dynamic systems
% [x, P] = ukf(f,x,P,h,z,Q,R) returns state estimate, x and state covariance, P
% for nonlinear dynamic system (for simplicity, noises are assumed as additive): % x_k+1 = f(x_k) + w_k
% z_k = h(x_k) + v_k
% where w ~ N(0,Q) meaning w is gaussian noise with covariance Q
% v ~ N(0,R) meaning v is gaussian noise with covariance R
% Inputs: f: function handle for f(x)
% x: "a priori" state estimate
% P: "a priori" estimated state covariance
% h: fanction handle for h(x)
% z: current measurement
% Q: process noise covariance
% R: measurement noise covariance
% Output: x: "a posteriori" state estimate
% P: "a posteriori" state covariance
%
% Example:
%{
n=3; %number of state
q=0.1; %std of process
r=0.1; %std of measurement
Q=q^2*eye(n); % covariance of process
R=r^2; % covariance of measurement
f=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))]; % nonlinear state equations
h=@(x)x(1); % measurement equation
s=[0;0;1]; % initial state
x=s+q*randn(3,1); %initial state % initial state with noise
P = eye(n); % initial state covraiance
N=20; % total dynamic steps
xV = zeros(n,N); %estmate % allocate memory
sV = zeros(n,N); %actual
zV = zeros(1,N);
for k=1:N
z = h(s) + r*randn; % measurments
sV(:,k)= s; % save actual state
zV(k) = z; % save measurment
[x, P] = ukf(f,x,P,h,z,Q,R); % ekf
xV(:,k) = x; % save estimate
s = f(s) + q*randn(3,1); % update process
end
for k=1:3 % plot results
subplot(3,1,k)
plot(1:N, sV(k,:), '-', 1:N, xV(k,:), '--')
end
%}
%
% By Yi Cao at Cranfield University, 04/01/2008
%
L=numel(x); %numer of states
m=numel(z); %numer of measurements
alpha=1e-3; %default, tunable
ki=0; %default, tunable
beta=2; %default, tunable
lambda=alpha^2*(L+ki)-L; %scaling factor
c=L+lambda; %scaling factor
Wm=[lambda/c 0.5/c+zeros(1,2*L)]; %weights for means
Wc=Wm;
Wc(1)=Wc(1)+(1-alpha^2+beta); %weights for covariance
c=sqrt(c);
X=sigmas(x,P,c); %sigma points around x
[x1,X1,P1,X2]=ut(fstate,X,Wm,Wc,L,Q); %unscented transformation of process % X1=sigmas(x1,P1,c); %sigma points around x1
% X2=X1-x1(:,ones(1,size(X1,2))); %deviation of X1
[z1,Z1,P2,Z2]=ut(hmeas,X1,Wm,Wc,m,R); %unscented transformation of measurments
P12=X2*diag(Wc)*Z2'; %transformed cross-covariance
K=P12*inv(P2);
x=x1+K*(z-z1); %state update
P=P1-K*P12'; %covariance update
function [y,Y,P,Y1]=ut(f,X,Wm,Wc,n,R)
%Unscented Transformation
%Input:
% f: nonlinear map
% X: sigma points
% Wm: weights for mean
% Wc: weights for covraiance
% n: numer of outputs of f
% R: additive covariance
%Output:
% y: transformed mean
% Y: transformed smapling points
% P: transformed covariance
% Y1: transformed deviations
L=size(X,2);
y=zeros(n,1);
Y=zeros(n,L);
for k=1:L
Y(:,k)=f(X(:,k));
y=y+Wm(k)*Y(:,k);
end
Y1=Y-y(:,ones(1,L));
P=Y1*diag(Wc)*Y1'+R;
function X=sigmas(x,P,c)
%Sigma points around reference point
%Inputs:
% x: reference point
% P: covariance
% c: coefficient
%Output:
% X: Sigma points
A = c*chol(P)';
Y = x(:,ones(1,numel(x))); X = [x Y+A Y-A];
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都是离散型的）。下面是扩展卡尔曼滤波器的相关公式。
一个应用实例详解卡尔曼滤波及其算法实现 标签：算法filtermatlabalgorithm优化工作 2012-05-14 10:48 75511人阅读评论(25) 收藏举报分类： 数据结构及其算法（4） 为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的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^2=5^2/(5^2+4^2)，所以Kg=0.78，我们可以估算出k时刻的实际温度值是：23+0.78*(25-23)=24.56度。 可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。 现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.56 度）的偏差。算法如下：((1-Kg)*5^2)^0.5=2.35。这里的5就是上面的k时刻你预测的那个23度
Kalman_Filter(float Gyro,float Accel) { Angle+=(Gyro - Q_bias) * dt; Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; Pdot[1]= - PP[1][1]; Pdot[2]= - PP[1][1]; Pdot[3]=Q_gyro; PP[0][0] += Pdot[0] * dt; PP[0][1] += Pdot[1] * dt; PP[1][0] += Pdot[2] * dt; PP[1][1] += Pdot[3] * dt; Angle_err = Accel - Angle; PCt_0 = C_0 * PP[0][0]; PCt_1 = C_0 * PP[1][0]; E = R_angle + C_0 * PCt_0; K_0 = PCt_0 / E; K_1 = PCt_1 / E; t_0 = PCt_0; t_1 = C_0 * PP[0][1]; PP[0][0] -= K_0 * t_0; PP[0][1] -= K_0 * t_1; PP[1][0] -= K_1 * t_0; PP[1][1] -= K_1 * t_1; Angle += K_0 * Angle_err; Q_bias += K_1 * Angle_err; Gyro_x = Gyro - Q_bias; } 首先是卡尔曼滤波的5个方程： -=--+（1）先验估计 X k k AX k k Bu k (|1)(1|1)() -=--+（2）协方差矩阵的预测(|1)(1|1)' P k k AP k k A Q
文件一 % 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;
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)
目录 一、kalman滤波简介 (2) 二、kalman滤波基本原理 (2) 三、Kalman滤波在运动跟踪中的应用的建模 (4) 四、仿真结果 (7) 1、kalman的滤波效果 (7) 2、简单轨迹的kalman的预测效果 (8) 3、椭圆运动轨迹的预测 (10) 4、往返运动归轨迹的预测 (11) 五、参数的选取 (12) 附录： (14) Matlab程序： (14) C语言程序： (14)
Kalman滤波在运动跟踪中的应用 一、kalman滤波简介 最佳线性滤波理论起源于40年代美国科学家Wiener和前苏联科学家Ｋолмогоров等人的研究工作，后人统称为维纳滤波理论。从理论上说，维纳滤波的最大缺点是必须用到无限过去的数据，不适用于实时处理。为了克服这一缺点，60年代Kalman把状态空间模型引入滤波理论，并导出了一套递推估计算法，后人称之为卡尔曼滤波理论。卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。它适合于实时处理和计算机运算。 Kalman滤波是卡尔曼(R.E.kalman)于1960年提出的从与被提取信号的有关的观测量中通过算法估计出所需信号的一种滤波算法。他把状态空间的概念引入到随机估计理论中，把信号过程视为白噪声作用下的—个线性系统的输出，用状方程来描述这种输入—输出关系，估计过程中利用系统状态方程、观测方程和白噪声激励(系统噪声和观测噪声)的统计特性形成滤波算法，由于所用的信息都是时域内的量，所以不但可以对平稳的一维随机过程进估计，也可以对非平稳的、多维随机过程进行估汁。 Kalman滤波是一套由计算机实现的实时递推算法．它所处理的对象是随机信号，利用系统噪声和观测噪声的统计特性，以系统的观测量作为滤波器的输入，以所要估计值(系统的状态或参数)作为滤波器的输出，滤波器的输入与输出之间是由时间更新和观测更新算法联系在一起的，根据系统方程和观测方程估计出所有需要处理的信号。所以，Kalman滤波与常规滤波的涵义与方法不同，它实质上是一种最优估计法。 卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法），对于解决很大部分的问题，他是最优，效率最高甚至是最有用的 二、kalman滤波基本原理 Kalman滤波器是目标状态估计算法解决状态最优估计的一种常用方法具有计算量小、存储量低、实时性高的优点。实际应用中，可以将物理系统的运行过程看作是一个状态转换过程，卡尔曼滤波将状态空间理论引入到对物理系统的数学建模过程中来。其基本思想是给系统信号和噪声的状态空间建立方程和观测方程，只用信号的前一个估计值和最近一个观察值就可以在线性无偏最小方差估计准则下对信号的当前值做出最优估计。 设一系统所建立的模型为：
卡尔曼滤波器及其简matlab仿真
卡尔曼滤波器及其简matlab仿真 一、卡尔曼滤波的起源 谈到信号的分析与处理，就离不开滤波两个字。通常，信号的频谱处于有限的频率范围内，而噪声的频谱则散布在很广的频率范围内，为了消除噪声，可以进行频域滤波。但在许多应用场合，需要直接进行时域滤波，从带噪声的信号中提取有用信号。虽然这样的过程其实也算是对信号的滤波，但其所依据的理论，即针对随机信号的估计理论，是自成体系的。人们对于随机信号干扰下的有用信号不能“确知”，只能“估计”。为了“估计”，要事先确定某种准则以评定估计的好坏程度。 1960年卡尔曼发表了用递归方法解决离散数据线性滤波问题的论文A New Approach to Linear Filtering and Prediction Problems （线性滤波与预测问题的新方法），在这篇文章里一种克服了维纳滤波缺点的新方法被提出来，这就是我们今天称之为卡尔曼滤波的方法。卡尔曼滤波应用广泛且功能强大，它可以估计信号的过去和当前状态甚至能估计将来的状态即使并不知道模型的确切性质。 其基本思想是以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值。算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。 对于解决很大部分的问题，它是最优，效率最高甚至是最有用的。它的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。 卡尔曼滤波不要求保存过去的测量数据，当新的数据到来时，根据新的数据和前一时刻的储值的估计，借助于系统本身的状态转移方程，按照一套递推公式，即可算出新的估值。卡尔曼递推算法大大减少了滤波装置的存储量和计算量，并且突破了平稳随机过程的限制，使卡尔曼滤波器适用于对时变信号的实时处理。
卡尔曼滤波简介及其算法实现代码 卡尔曼滤波算法实现代码（C，C＋＋分别实现） 卡尔曼滤波器简介 近来发现有些问题很多人都很感兴趣。所以在这里希望能尽自己能力跟大家讨论一些力所能及的算法。现在先讨论一下卡尔曼滤波器，如果时间和能力允许，我还希望能够写写其他的算法，例如遗传算法，傅立叶变换，数字滤波，神经网络，图像处理等等。 因为这里不能写复杂的数学公式，所以也只能形象的描述。希望如果哪位是这方面的专家，欢迎讨论更正。 卡尔曼滤波器– 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.doczj.com/doc/e518243747.html,/~welch/media/pdf/Kalman1960.pdf。 简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。 2．卡尔曼滤波器的介绍 （Introduction to the Kalman Filter） 为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。 在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。 假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就
目录 一、kalman滤波简介 (1) 二、kalman滤波基本原理 (1) 三、Kalman滤波在运动跟踪中的应用的建模 (3) 四、仿真结果 (6) 1、kalman的滤波效果 (6) 2、简单轨迹的kalman的预测效果 (7) 3、椭圆运动轨迹的预测 (9) 4、往返运动归轨迹的预测 (10) 五、参数的选取 (11) 附录： (13) Matlab程序： (13) C语言程序： (13)
Kalman滤波在运动跟踪中的应用 一、kalman滤波简介 最佳线性滤波理论起源于40年代美国科学家Wiener和前苏联科学家Ｋолмогоров等人的研究工作，后人统称为维纳滤波理论。从理论上说，维纳滤波的最大缺点是必须用到无限过去的数据，不适用于实时处理。为了克服这一缺点，60年代Kalman把状态空间模型引入滤波理论，并导出了一套递推估计算法，后人称之为卡尔曼滤波理论。卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。它适合于实时处理和计算机运算。 Kalman滤波是卡尔曼(R.E.kalman)于1960年提出的从与被提取信号的有关的观测量中通过算法估计出所需信号的一种滤波算法。他把状态空间的概念引入到随机估计理论中，把信号过程视为白噪声作用下的—个线性系统的输出，用状方程来描述这种输入—输出关系，估计过程中利用系统状态方程、观测方程和白噪声激励(系统噪声和观测噪声)的统计特性形成滤波算法，由于所用的信息都是时域内的量，所以不但可以对平稳的一维随机过程进估计，也可以对非平稳的、多维随机过程进行估汁。 Kalman滤波是一套由计算机实现的实时递推算法．它所处理的对象是随机信号，利用系统噪声和观测噪声的统计特性，以系统的观测量作为滤波器的输入，以所要估计值(系统的状态或参数)作为滤波器的输出，滤波器的输入与输出之间是由时间更新和观测更新算法联系在一起的，根据系统方程和观测方程估计出所有需要处理的信号。所以，Kalman滤波与常规滤波的涵义与方法不同，它实质上是一种最优估计法。 卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法），对于解决很大部分的问题，他是最优，效率最高甚至是最有用的 二、kalman滤波基本原理 Kalman滤波器是目标状态估计算法解决状态最优估计的一种常用方法具有计算量小、存储量低、实时性高的优点。实际应用中，可以将物理系统的运行过程看作是一个状态转换过程，卡尔曼滤波将状态空间理论引入到对物理系统的数学建模过程中来。其基本思想是给系统信号和噪声的状态空间建立方程和观测方程，只用信号的前一个估计值和最近一个观察值就可以在线性无偏最小方差估计准则下对信号的当前值做出最优估计。 设一系统所建立的模型为：
卡尔曼滤波器及其简matlab仿真 一、卡尔曼滤波的起源 谈到信号的分析与处理，就离不开滤波两个字。通常，信号的频谱处于有限的频率范围内，而噪声的频谱则散布在很广的频率范围内，为了消除噪声，可以进行频域滤波。但在许多应用场合，需要直接进行时域滤波，从带噪声的信号中提取有用信号。虽然这样的过程其实也算是对信号的滤波，但其所依据的理论，即针对随机信号的估计理论，是自成体系的。人们对于随机信号干扰下的有用信号不能“确知”，只能“估计”。为了“估计”，要事先确定某种准则以评定估计的好坏程度。 1960年卡尔曼发表了用递归方法解决离散数据线性滤波问题的论文A New Approach to Linear Filtering and Prediction Problems（线性滤波与预测问题的新方法），在这篇文章里一种克服了维纳滤波缺点的新方法被提出来，这就是我们今天称之为卡尔曼滤波的方法。卡尔曼滤波应用广泛且功能强大，它可以估计信号的过去和当前状态甚至能估计将来的状态即使并不知道模型的确切性质。 其基本思想是以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值。算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。 对于解决很大部分的问题，它是最优，效率最高甚至是最有用的。它的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。 卡尔曼滤波不要求保存过去的测量数据，当新的数据到来时，根据新的数据和前一时刻的储值的估计，借助于系统本身的状态转移方程，按照一套递推公式，即可算出新的估值。卡尔曼递推算法大大减少了滤波装置的存储量和计算量，并且突破了平稳随机过程的限制，使卡尔曼滤波器适用于对时变信号的实时处理。 二、卡尔曼滤波的原理
扩展卡尔曼滤波器教程 在使用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 试着移动上面的滑块看到噪声对观察到的高度的影响。噪音被表示为可观测的海拔范围的百分比。
clear all v=150; %%目标速度 v_sensor=0;%%传感器速度 t=1; %%扫描周期 xradarpositon=0; %%传感器坐标yradarpositon=0; %% ppred=zeros(4,4); Pzz=zeros(2,2); Pxx=zeros(4,2); xpred=zeros(4,1); ypred=zeros(2,1); sumx=0; sumy=0; sumxukf=0; sumyukf=0; sumxekf=0; sumyekf=0; %%%统计的初值 L=4; alpha=1; kalpha=0; belta=2; ramda=3-L; azimutherror=0.015; %%方位均方误差rangeerror=100; %%距离均方误差processnoise=1; %%过程噪声均方差 tao=[t^3/3 t^2/2 0 0; t^2/2 t 0 0; 0 0 t^3/3 t^2/2; 0 0 t^2/2 t]; %% the input matrix of process G=[t^2/2 0 t 0 0 t^2/2 0 t ]; a=35*pi/180; a_v=5/100; a_sensor=45*pi/180; x(1)=8000; %%初始位置
y(1)=12000; for i=1:200 x(i+1)=x(i)+v*cos(a)*t; y(i+1)=y(i)+v*sin(a)*t; end for i=1:200 xradarpositon=0; yradarpositon=0; Zmeasure(1,i)=atan((y(i)-yradarpositon)/(x(i)-xradarpositon))+random('Normal',0,azimutherror,1,1); Zmeasure(2,i)=sqrt((y(i)-yradarpositon)^2+(x(i)-xradarpositon)^2)+random('Normal',0,rangeerror,1,1); xx(i)=Zmeasure(2,i)*cos(Zmeasure(1,i));%%观测值 yy(i)=Zmeasure(2,i)*sin(Zmeasure(1,i)); measureerror=[azimutherror^2 0;0 rangeerror^2]; processerror=tao*processnoise; vNoise = size(processerror,1); wNoise = size(measureerror,1); A=[1 t 0 0; 0 1 0 0; 0 0 1 t; 0 0 0 1]; Anoise=size(A,1); for j=1:2*L+1 Wm(j)=1/(2*(L+ramda)); Wc(j)=1/(2*(L+ramda)); end Wm(1)=ramda/(L+ramda); Wc(1)=ramda/(L+ramda);%+1-alpha^2+belta; %%%权值 if i==1 xerror=rangeerror^2*cos(Zmeasure(1,i))^2+Zmeasure(2,i)^2*azimutherror^2*sin(Zmeasure(1,i))^2; yerror=rangeerror^2*sin(Zmeasure(1,i))^2+Zmeasure(2,i)^2*azimutherror^2*cos(Zmeasure(1,i))^2; xyerror=(rangeerror^2-Zmeasure(2,i)^2*azimutherror^2)*sin(Zmeasure(1,i))*cos(Zmeasure(1,i)); P=[xerror xerror/t xyerror xyerror/t; xerror/t 2*xerror/(t^2) xyerror/t 2*xyerror/(t^2); xyerror xyerror/t yerror yerror/t;
自适应卡尔曼滤波 卡尔曼滤波发散的原因 如果卡尔曼滤波是稳定的，随着滤波的推进，卡尔曼滤波估计的精度应该越来越高，滤波误差方差阵也应趋于稳定值或有界值。但在实际应用中，随着量测值数目的增加，由于估计误差的均值和估计误差协方差可能越来越大，使滤波逐渐失去准确估计的作用，这种现象称为卡尔曼滤波发散。 引起滤波器发散的主要原因有两点： （1）描述系统动力学特性的数学模型和噪声估计模型不准确，不能直接真实地反映物理过程，使得模型与获得的量测值不匹配而导致滤波发散。这种由于模型建立过于粗糙或失真所引起的发散称为滤波发散。 （2）由于卡尔曼滤波是递推过程，随着滤波步数的增加，舍入误差将逐渐积累。如果计算机字长不够长，这种积累误差很有可能使估计误差方差阵失去非负定性甚至失去对称性，使滤波增益矩阵逐渐失去合适的加权作用而导致发散。这种由于计算舍入误差所引起的发散称为计算发散。 针对上述卡尔曼滤波发散的原因，目前已经出现了几种有效抑制滤波发散的方法，常用的有衰减记忆滤波、限定记忆滤波、扩充状态滤波、有限下界滤波、平方根滤波、和自适应滤波等。这些方法本质上都是以牺牲滤波器的最优性为代价来抑制滤波发散，也就是说，多数都是次优滤波方法。 自适应滤波 在很多实际系统中，系统过程噪声方差矩阵Q和量测误差方差阵R事先是不知道的，有时甚至连状态转移矩阵 或量测矩阵H也不能确切建立。如果所建立的模型与实际模型不符可能回引起滤波发散。自适应滤波就是这样一种具有抑制滤波发散作用的滤波方法。在滤波过程中，自适应滤波一方面利用量测值修正预测值，同时也对未知的或不确切的系统模型参数和噪声统计参数进行估计修正。自适应滤波的方法很多，包括贝叶斯法、极大似然法、相关法与协方差匹配法，其中最基本也是最重要的是相关法，而相关法可分为输出相关法和新息相关法。 在这里只讨论系统模型参数已知，而噪声统计参数Q和R未知情况下的自适应滤波。由于Q和R等参数最终是通过增益矩阵K影响滤波值的，因此进行自适应滤波时，也可以不去估计Q和R等参数而直接根据量测数据调整K就可以了。
卡尔曼滤波器综述 瞿伟军 G10074 1、卡尔曼滤波的起源 1960年，匈牙利数学家卡尔曼发表了一篇关于离散数据线性滤波递推算法的论文，这意味着卡尔曼滤波的诞生。斯坦利.施密特(Stanley Schmidt)首次实现了卡尔曼滤波器，卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑使用了这种滤波器。关于这种滤波器的论文由Swerling (1958)、Kalman (1960)与 Kalman and Bucy (1961)发表。 2、卡尔曼滤波的发展 卡尔曼滤波是一种有着相当广泛应用的滤波方法，但它既需要假定系统是线性的，又需要认为系统中的各个噪声与状态变量均呈高斯分布，而这两条并不总是确切的假设限制了卡尔曼滤波器在现实生活中的应用。扩展卡尔曼滤波器（EKF）极大地拓宽了卡尔曼滤波的适用范围。EKF的基本思路是，假定卡尔曼滤滤对当前系统状态估计值非常接近于其真实值，于是将非线性函数在当前状态估计值处进行台劳展开并实现线性化。另一种非线性卡尔曼滤波叫线性化卡尔曼滤波。它与EKF的主要区别是前者将非线函数在滤波器对当前系统状态的最优估计值处线性化，而后者因为预先知道非线性系统的实际运行状态大致按照所要求、希望的轨迹变化，所以这些非线性化函数在实际状态处的值可以表达为在希望的轨迹处的台劳展开式，从而完成线性化。 不敏卡尔曼滤波器（UKF）是针对非线性系统的一种改进型卡尔曼滤波器。UKF处理非线性系统的基本思路在于不敏变换，而不敏变换从根本上讲是一种描述高斯随机变量在非线性化变换后的概率分布情况的方法。不敏卡尔曼滤波认为，与其将一个非线性化变换线性化、近似化，还不如将高斯随机变量经非线性变换后的概率分布情况用高斯分布来近似那样简单，因而不敏卡尔曼滤波算法没
第十三章 卡尔曼滤波 在本章中，我们介绍一种被称为卡尔曼滤波的十分有用的工具。卡尔曼滤波的基本思想是将动态系统表示成为一种称为状态空间表示的特殊情形。卡尔曼滤波是对系统线性投影进行序列更新的算法。除了一般的优点以外，这种算法对计算确切的有限样本预测、计算Gauss ARMA 模型的确切似然函数、估计具有时变参数的自回归模型等，都提供了重要方法。 §13.1 动态系统的状态空间表示 我们已经介绍过一些随机过程的动态表示方法，下面我们在以前的假设基础上，继续分析动态系统的表示方法。 13.1.1 继续使用的假设 假设t y 表示时刻t 观测到的n 维随机向量，一类非常丰富的描述t y 动态性的模型可以利用一些可能无法观测的被称为状态向量(state vector)的r 维向量t ξ表示，因此表示t y 动态性的状态空间表示(state-space representation)由下列方程系统给出： 11+++=t t t v ξF ξ 状态方程(state model) (13.1) t t t w ξH x A y t +'+'= 量测方程(observation model) (13.2) 这里F ，A '和H '分别是阶数为r r ?，k n ?和r n ?的参数矩阵，t x 是1?k 的外生或者前定变量。方程(13.1)被称为状态方程(state model)，方程(13.2)被称为量测方程(observation model)，1?r 维向量t v 和1?n 维向量t w 都是向量白噪声，满足： ? ??≠=='τττt t E t ,,)(0Q v v (13.3) ? ??≠=='τττt t E t ,,)(0R w w (13.4) 这里Q 和R 是r r ?和n n ?阶矩阵。假设扰动项t v 和t w 对于所有阶滞后都是不相关的，即对所有t 和τ，有： 0w v =')(τ t E (13.5) t x 是外生或者前定变量的假定意味着，在除了包含在121,,,y y y --t t 内的信息以外，t x 没有为s t +ξ和s t +w ( ,2,1,0=s )提供任何新的信息。例如，t x 可以包括t y 的滞后值，也可以包括与τξ和τw (任意τ)不相关的变量。 方程系统中方程(13.1)至方程(13.5)可以表示有限观测值的序列},,,{21T y y y ，这时需要状态向量初始值1ξ。假设1ξ与t v 和t w 的任何实现都不相关： 0ξv =')(1 t E ，对任意T t ,,2,1 = (13.6) 0ξw =')(1 t E ，对任意T t ,,2,1 = (13.7) 状态方程(13.1)表明，t ξ可以表示成为},,,,{321t v v v ξ 的线性函数： 1122221ξF v F v F v F v ξ----+++++=t t t t t t ，T t ,,3,2 = (13.8) 因此，方程(13.6)和方程(13.3)意味着t v 与所有ξ的滞后值都是不相关的： 0ξv =')(τ t E ，1,,2,1 --=t t τ (13.9) 类似地，可以得到： 0ξw =')(τ t E ，T ,,2,1 =τ (13.10)
% 扩展卡尔曼滤波器估计单相电压幅值、相位、频率参数（含直流）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代表过程激励噪声和观测噪声。它们为相互独立，服从正态分布的白色噪声：
卡尔曼滤波的基本原理及应用卡尔曼滤波在信号处理与系统控制领域应用广泛，目前，正越来越广泛地应用于计算机应用的各个领域。为了更好地理解卡尔曼滤波的原理与进行滤波算法的设计工作，主要从两方面对卡尔曼滤波进行阐述：基本卡尔曼滤波系统模型、滤波模型的建立以及非线性卡尔曼滤波的线性化。最后，对卡尔曼滤波的应用做了简单介绍。 卡尔曼滤波属于一种软件滤波方法，其基本思想是：以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型，利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值，算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。 最初的卡尔曼滤波算法被称为基本卡尔曼滤波算法，适用于解决随机线性离散系统的状态或参数估计问题。卡尔曼滤波器包括两个主要过程：预估与校正。预估过程主要是利用时间更新方程建立对当前状态的先验估计，及时向前推算当前状态变量和误差协方差估计的值，以便为下一个时间状态构造先验估计值；校正过程负责反馈，利用测量更新方程在预估过程的先验估计值及当前测量变量的基础上建立起对当前状态的改进的后验估计。这样的一个过程，我们称之为预估－校正过程，对应的这种估计算法称为预估－校正算法。以下给出离散卡尔曼滤波的时间更新方程和状态更新方程。 时间更新方程： 状态更新方程： 在上面式中，各量说明如下： A：作用在X k-1上的n×n 状态变换矩阵 B：作用在控制向量U k-1上的n×1 输入控制矩阵 H：m×n 观测模型矩阵，它把真实状态空间映射成观测空间 P k－：为n×n 先验估计误差协方差矩阵 P k：为n×n 后验估计误差协方差矩阵 Q：n×n 过程噪声协方差矩阵 R：m×m 过程噪声协方差矩阵 I：n×n 阶单位矩阵K k：n×m 阶矩阵，称为卡尔曼增益或混合因数 随着卡尔曼滤波理论的发展，一些实用卡尔曼滤波技术被提出来，如自适应滤波，次优滤波以及滤波发散抑制技术等逐渐得到广泛应用。其它的滤波理论也迅速发展，如线性离散系统的分解滤波（信息平方根滤波，序列平方根滤波，UD 分解滤波），鲁棒滤波（H∞波）。 非线性样条自适应滤波：这是一类新的非线性自适应滤波器，它由一个线性组合器后跟挠性无记忆功能的。涉及的自适应处理的非线性函数是基于可在学习