搜档网
当前位置:搜档网 › 卡尔曼滤波简介及其算法实现代码(C++_C_MATLAB)

卡尔曼滤波简介及其算法实现代码(C++_C_MATLAB)

卡尔曼滤波简介及其算法实现代码(C++_C_MATLAB)
卡尔曼滤波简介及其算法实现代码(C++_C_MATLAB)

卡尔曼滤波简介及其算法实现代码(C++/C/MATLAB)

卡尔曼滤波器简介

近来发现有些问题很多人都很感兴趣。所以在这里希望能尽自己能力跟大家讨论一些力所能及的算法。现在先讨论一下卡尔曼滤波器,如果时间和能力允许,我还希望能够写写其他的算法,例如遗传算法,傅立叶变换,数字滤波,神经网络,图像处理等等。

因为这里不能写复杂的数学公式,所以也只能形象的描述。希望如果哪位是这方面的专家,欢迎讨论更正。

卡尔曼滤波器– 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.sodocs.net/doc/a712334727.html,/~welch/media/pdf/Kalman1960.pdf。

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

2.卡尔曼滤波器的介绍

(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^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度温度值的偏差,得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差(对应于上面的3)。

就是这样,卡尔曼滤波器就不断的把covariance递归,从而估算出最优的温度值。他运行的很快,而且它只保留了上一时刻的covariance。上面的Kg,就是卡尔曼增益(Kalman Gain)。他可以随不同的时刻而改变他自己的值,是不是很神奇!

下面就要言归正传,讨论真正工程系统上的卡尔曼。

3.卡尔曼滤波器算法

(The Kalman Filter Algorithm)

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

首先,我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程(Linear Stochastic Difference equation)来描述:

X(k)=A X(k-1)+B U(k)+W(k)

再加上系统的测量值:

Z(k)=H X(k)+V(k)

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

对于满足上面的条件(线性随机微分系统,过程和测量都是高斯白噪声),卡尔曼滤波器是最优的信息处理器。下面我们来用他们结合他们的covariances 来估算系统的最优化输出(类似上一节那个温度的例子)。

首先我们要利用系统的过程模型,来预测下一状态的系统。假设现在的系统状态是k,根据系统的模型,可以基于系统的上一状态而预测出现在状态:

X(k|k-1)=A X(k-1|k-1)+B U(k) (1)

式(1)中,X(k|k-1)是利用上一状态预测的结果,X(k-1|k-1)是上一状态最优的结果,U(k)为现在状态的控制量,如果没有控制量,它可以为0。

到现在为止,我们的系统结果已经更新了,可是,对应于X(k|k-1)的covariance 还没更新。我们用P表示covariance:

P(k|k-1)=A P(k-1|k-1) A’+Q (2)

式 (2)中,P(k|k-1)是X(k|k-1)对应的covariance,P(k-1|k-1)是X(k-1|k-1)对应的 covariance,A’表示A的转置矩阵,Q是系统过程的covariance。式子1,2就是卡尔曼滤波器5个公式当中的前两个,也就是对系统的预测。

现在我们有了现在状态的预测结果,然后我们再收集现在状态的测量值。结合预测值和测量值,我们可以得到现在状态(k)的最优化估算值X(k|k):

X(k|k)= X(k|k-1)+Kg(k)*(Z(k)-H X(k|k-1)) (3)

其中Kg为卡尔曼增益(Kalman Gain):

Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) (4)

到现在为止,我们已经得到了k状态下最优的估算值X(k|k)。但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束,我们还要更新k状态下X(k|k)的covariance:

P(k|k)=(I-Kg(k) H)P(k|k-1) (5)

其中I 为1的矩阵,对于单模型单测量,I=1。当系统进入k+1状态时,P(k|k)

就是式子(2)的P(k-1|k-1)。这样,算法就可以自回归的运算下去。

卡尔曼滤波器的原理基本描述了,式子1,2,3,4和5就是他的5 个基本公式。根据这5个公式,可以很容易的实现计算机的程序。

下面,我会用程序举一个实际运行的例子。。。

4.简单例子

(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)

现在我们模拟一组测量值作为输入。假设房间的真实温度为25度,我模拟了200个测量值,这些测量值的平均值为25度,但是加入了标准偏差为几度的高斯白噪声(在图中为蓝线)。

为了令卡尔曼滤波器开始工作,我们需要告诉卡尔曼两个零时刻的初始值,是X(0|0)和P(0|0)。他们的值不用太在意,随便给一个就可以了,因为随着卡尔曼的工作,X会逐渐的收敛。但是对于P,一般不要取0,因为这样可能会令卡尔曼完全相信你给定的X(0|0)是系统最优的,从而使算法不能收敛。我选了

X(0|0)=1度,P(0|0)=10。

该系统的真实温度为25度,图中用黑线表示。图中红线是卡尔曼滤波器输出的最优化结果(该结果在算法中设置了Q=1e-6,R=1e-1)。

最佳线性滤波理论起源于 40 年代美国科学家 Wiener 和前苏联科学家K

олмогоров等人的研究工作,后人统称为维纳滤波理论。从理论上说,维纳滤波的最大缺点是必须用到无限过去的数据,不适用于实时处理。为了克服这一缺点, 60 年代 Kalman 把状态空间模型引入滤波理论,并导出了一套递推估计算法,后人称之为卡尔曼滤波理论。卡尔曼滤波是以最小均方误差为估计的最佳准则,来寻求一套递推估计的算法,其基本思想是:采用信号与噪

声的状态空间模型,利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计,求出现时刻的估计值。它适合于实时处理和计算机运算。

现设线性时变系统的离散状态防城和观测方程为:

X(k) = F(k,k-1)·X(k-1)+T(k,k-1)·U(k-1)

Y(k) = H(k)·X(k)+N(k)

其中,X(k)和Y(k)分别是k时刻的状态矢量和观测矢量

F(k,k-1)为状态转移矩阵

U(k)为k时刻动态噪声

T(k,k-1)为系统控制矩阵

H(k)为k时刻观测矩阵

N(k)为k时刻观测噪声

则卡尔曼滤波的算法流程为:

预估计X(k)^= F(k,k-1)·X(k-1)

1.计算预估计协方差矩阵

C(k)^=F(k,k-1)×C(k)×F(k,k-1)'+T(k,k-1)×Q(k)×T(k,k-1)'

Q(k) = U(k)×U(k)'

2.计算卡尔曼增益矩阵

K(k) = C(k)^×H(k)'×[H(k)×C(k)^×H(k)'+R(k)]^(-1)

R(k) = N(k)×N(k)'

3.更新估计

X(k)~=X(k)^+K(k)×[Y(k)-H(k)×X(k)^]

4.计算更新后估计协防差矩阵

C(k)~ =

[I-K(k)×H(k)]×C(k)^×[I-K(k)×H(k)]'+K(k)×R(k)×K(k)'

5.X(k+1) = X(k)~

C(k+1) = C(k)~

重复以上步骤

**********************************************

Matlab实现代码[此码为本人原创,仅供交流,谢决转载!]

********************************************************************* ************************************************************

%%%% Constant Velocity Model Kalman Filter Simulation %%%%

%==================================================================== ======

clear all; close all; clc;

%% Initial condition 初始条件

ts = 1; % Sampling time

t = [0:ts:100];

T = length(t);%T表示t的长度101

%% Initial state 初始状态

x = [0 40 0 20]';

x_hat = [0 0 0 0]';

%% Process noise covariance 预测噪声的协方差

q = 5

Q = q*eye(2);

%% Measurement noise covariance 测量噪声的协方差

r = 5

R = r*eye(2);

%% Process and measurement noise 预测以及测量的噪声

w = sqrt(Q)*randn(2,T); % Process noise

v = sqrt(R)*randn(2,T); % Measurement noise

%% Estimate error covariance initialization 预测协方差初始化

p = 5;

P(:,:,1) = p*eye(4);

%==================================================================== ======

%% Continuous-time state space model 状态空间模型

%{

x_dot(t) = Ax(t)+Bu(t)

z(t) = Cx(t)+Dn(t)

%}

A = [0 1 0 0;

0 0 0 0;

0 0 0 1;

0 0 0 0];

B = [0 0;

1 0;

0 0;

0 1];

C = [1 0 0 0;

0 0 1 0];

D = [1 0;

0 1];

%% Discrete-time state space model 离散状态模型

%{

x(k+1) = Fx(k)+Gw(k)

z(k) = Hx(k)+Iv(k)

Continuous to discrete form by zoh 连续转为离散

%}

sysc = ss(A,B,C,D);

sysd = c2d(sysc, ts, 'zoh');

[F G H I] = ssdata(sysd);

%% Practice state of target

for i = 1:T-1

x(:,i+1) = F*x(:,i);

end

x = x+G*w; % State variable with noise

z = H*x+I*v; % Measurement value with noise

%==================================================================== ======

%%% Kalman Filter

for i = 1:T-1

%% Prediction phase

x_hat(:,i+1) = F*x_hat(:,i);

% State estimate predict 预测模型

P(:,:,i+1) = F*P(:,:,i)*F'+G*Q*G';

% Tracking error covariance predict 预测协方差

P_predicted(:,:,i+1) = P(:,:,i+1);

%% Kalman gain 卡尔曼增益

K = P(:,:,i+1)*H'*inv(H*P(:,:,i+1)*H'+R);

%% Updata step 状态更新

x_hat(:,i+1) =

x_hat(:,i+1)+K*(z(:,i+1)-H*x_hat(:,i+1));

% State estimate update 协方差更新

P(:,:,i+1) = P(:,:,i+1)-K*H*P(:,:,i+1); % Tracking error covariance update

P_updated(:,:,i+1) = P(:,:,i+1); end

%==================================================================== ======

%% Estimate error 预测错误差值

x_error = x-x_hat;

%% Graph 1 practical and tracking position

figure(1)

plot(x(1,:),x(3,:),'r');

hold on;

plot(x_hat(1,:),x_hat(3,:),'g.');

title('2D Target Position')

legend('Practical Position','Tracking Position')

xlabel('X axis [m]')

ylabel('Y axis [m]')

hold off;

%% Graph 2

figure(2)

plot(t,x(1,:)),grid on;

hold on;

plot(t,x_hat(1,:),'r'),grid on;

title('Practical and Tracking Position on X axis')

legend('Practical Position','Tracking Position')

xlabel('Time [sec]')

ylabel('Position [m]')

hold off;

%% Graph 3

figure(3)

plot(t,x_error(1,:)),grid on;

title('Position Error on X axis')

xlabel('Time [sec]')

ylabel('Position RMSE [m]')

hold off;

%% Graph 4

figure(4)

plot(t,x(2,:)),grid on;

hold on;

plot(t,x_hat(2,:),'r'),grid on;

title('Practical and Tracking Velocity on X axis')

legend('Practical Velocity','Tracking Velocity')

xlabel('Time [sec]')

ylabel('Velocity [m/sec]')

hold off;

%% Graph 5

figure(5)

plot(t,x_error(2,:)),grid on;

title('Velocity Error on X axis')

xlabel('Time [sec]')

ylabel('Velocity RMSE [m/sec]')

hold off;

%==================================================================== ======

********************************************************************* ************************************************************

***********************************************

c语言实现代码[转]

-----------------------------------------------------------------------------------------------------------------------------

#include "stdlib.h"

#include "rinv.c"

int lman(n,m,k,f,q,r,h,y,x,p,g)

int n,m,k;

double f[],q[],r[],h[],y[],x[],p[],g[];

{ int i,j,kk,ii,l,jj,js;

double *e,*a,*b;

e=malloc(m*m*sizeof(double));

l=m;

if (l

a=malloc(l*l*sizeof(double));

b=malloc(l*l*sizeof(double));

for (i=0; i<=n-1; i++)

for (j=0; j<=n-1; j++)

{ ii=i*l+j; a[ii]=0.0;

for (kk=0; kk<=n-1; kk++)

a[ii]=a[ii]+p[i*n+kk]*f[j*n+kk];

}

for (i=0; i<=n-1; i++)

for (j=0; j<=n-1; j++)

{ ii=i*n+j; p[ii]=q[ii];

for (kk=0; kk<=n-1; kk++)

p[ii]=p[ii]+f[i*n+kk]*a[kk*l+j];

}

for (ii=2; ii<=k; ii++)

{ for (i=0; i<=n-1; i++)

for (j=0; j<=m-1; j++)

{ jj=i*l+j; a[jj]=0.0;

for (kk=0; kk<=n-1; kk++)

a[jj]=a[jj]+p[i*n+kk]*h[j*n+kk];

}

for (i=0; i<=m-1; i++)

for (j=0; j<=m-1; j++)

{ jj=i*m+j; e[jj]=r[jj];

for (kk=0; kk<=n-1; kk++)

e[jj]=e[jj]+h[i*n+kk]*a[kk*l+j];

}

js=rinv(e,m);

if (js==0)

{ free(e); free(a); free(b); return(js);} for (i=0; i<=n-1; i++)

for (j=0; j<=m-1; j++)

{ jj=i*m+j; g[jj]=0.0;

for (kk=0; kk<=m-1; kk++)

g[jj]=g[jj]+a[i*l+kk]*e[j*m+kk];

}

for (i=0; i<=n-1; i++)

{ jj=(ii-1)*n+i; x[jj]=0.0;

for (j=0; j<=n-1; j++)

x[jj]=x[jj]+f[i*n+j]*x[(ii-2)*n+j]; }

for (i=0; i<=m-1; i++)

{ jj=i*l; b[jj]=y[(ii-1)*m+i];

for (j=0; j<=n-1; j++)

b[jj]=b[jj]-h[i*n+j]*x[(ii-1)*n+j]; }

for (i=0; i<=n-1; i++)

{ jj=(ii-1)*n+i;

for (j=0; j<=m-1; j++)

x[jj]=x[jj]+g[i*m+j]*b[j*l];

}

if (ii

{ for (i=0; i<=n-1; i++)

for (j=0; j<=n-1; j++)

{ jj=i*l+j; a[jj]=0.0;

for (kk=0; kk<=m-1; kk++)

a[jj]=a[jj]-g[i*m+kk]*h[kk*n+j];

if (i==j) a[jj]=1.0+a[jj];

}

for (i=0; i<=n-1; i++)

for (j=0; j<=n-1; j++)

{ jj=i*l+j; b[jj]=0.0;

for (kk=0; kk<=n-1; kk++)

b[jj]=b[jj]+a[i*l+kk]*p[kk*n+j];

}

for (i=0; i<=n-1; i++)

for (j=0; j<=n-1; j++)

{ jj=i*l+j; a[jj]=0.0;

for (kk=0; kk<=n-1; kk++)

a[jj]=a[jj]+b[i*l+kk]*f[j*n+kk];

}

for (i=0; i<=n-1; i++)

for (j=0; j<=n-1; j++)

{ jj=i*n+j; p[jj]=q[jj];

for (kk=0; kk<=n-1; kk++)

p[jj]=p[jj]+f[i*n+kk]*a[j*l+kk];

}

}

}

free(e); free(a); free(b);

return(js);

}

********************************************************************* **************************************

C++实现代码[转]

--------------------------------------------------------------------------------------------------------------------------------------------------------

// kalman.h: interface for the kalman class.

//

///////////////////////////////////////////////////////////////////// /

#if !defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCL UDED_)

#define AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLUDED_

#if _MSC_VER > 1000

#pragma once

#endif // _MSC_VER > 1000

#include

#include "cv.h"

class kalman

{

public:

void init_kalman(int x,int xv,int y,int yv);

CvKalman* cvkalman;

CvMat* state;

CvMat* process_noise;

CvMat* measurement;

const CvMat* prediction;

CvPoint2D32f get_predict(float x, float y);

kalman(int x=0,int xv=0,int y=0,int yv=0);

//virtual ~kalman();

};

#endif

// !defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLU DED_)

============================kalman.cpp=============================== =

#include "kalman.h"

#include

/* tester de printer toutes les valeurs des vecteurs*/

/* tester de changer les matrices du noises */

/* replace state by cvkalman->state_post ??? */

CvRandState rng;

const double T = 0.1;

kalman::kalman(int x,int xv,int y,int yv)

{

cvkalman = cvCreateKalman( 4, 4, 0 );

state = cvCreateMat( 4, 1, CV_32FC1 );

process_noise = cvCreateMat( 4, 1, CV_32FC1 ); measurement = cvCreateMat( 4, 1, CV_32FC1 ); int code = -1;

/* create matrix data */

const float A[] = {

1, T, 0, 0,

0, 1, 0, 0,

0, 0, 1, T,

0, 0, 0, 1

};

const float H[] = {

1, 0, 0, 0,

0, 0, 0, 0,

0, 0, 1, 0,

0, 0, 0, 0

};

const float P[] = {

pow(320,2), pow(320,2)/T, 0, 0,

pow(320,2)/T, pow(320,2)/pow(T,2), 0, 0,

0, 0, pow(240,2), pow(240,2)/T,

0, 0, pow(240,2)/T, pow(240,2)/pow(T,2)

};

const float Q[] = {

pow(T,3)/3, pow(T,2)/2, 0, 0,

pow(T,2)/2, T, 0, 0,

0, 0, pow(T,3)/3, pow(T,2)/2,

0, 0, pow(T,2)/2, T

};

const float R[] = {

1, 0, 0, 0,

0, 0, 0, 0,

0, 0, 1, 0,

0, 0, 0, 0

};

cvRandInit( &rng, 0, 1, -1, CV_RAND_UNI );

cvZero( measurement );

cvRandSetRange( &rng, 0, 0.1, 0 );

rng.disttype = CV_RAND_NORMAL;

cvRand( &rng, state );

memcpy( cvkalman->transition_matrix->data.fl, A, sizeof(A));

memcpy( cvkalman->measurement_matrix->data.fl, H, sizeof(H));

memcpy( cvkalman->process_noise_cov->data.fl, Q, sizeof(Q));

memcpy( cvkalman->error_cov_post->data.fl, P, sizeof(P));

memcpy( cvkalman->measurement_noise_cov->data.fl, R, sizeof(R)); //cvSetIdentity( cvkalman->process_noise_cov,

cvRealScalar(1e-5) );

//cvSetIdentity( cvkalman->error_cov_post, cvRealScalar(1));

//cvSetIdentity( cvkalman->measurement_noise_cov,

cvRealScalar(1e-1) );

/* choose initial state */

state->data.fl[0]=x;

state->data.fl[1]=xv;

state->data.fl[2]=y;

state->data.fl[3]=yv;

cvkalman->state_post->data.fl[0]=x;

cvkalman->state_post->data.fl[1]=xv;

cvkalman->state_post->data.fl[2]=y;

cvkalman->state_post->data.fl[3]=yv;

cvRandSetRange( &rng, 0, sqrt(cvkalman->process_noise_cov->data.fl[0]), 0 );

cvRand( &rng, process_noise );

}

CvPoint2D32f kalman::get_predict(float x, float y){

/* update state with current position */

state->data.fl[0]=x;

state->data.fl[2]=y;

/* predict point position */

/* x'k=A鈥 k+B鈥 k

P'k=A鈥 k-1*AT + Q */

cvRandSetRange( &rng, 0,

sqrt(cvkalman->measurement_noise_cov->data.fl[0]), 0 );

cvRand( &rng, measurement );

/* xk=A?xk-1+B?uk+wk */

cvMatMulAdd( cvkalman->transition_matrix, state, process_noise, cvkalman->state_post );

/* zk=H?xk+vk */

cvMatMulAdd( cvkalman->measurement_matrix, cvkalman->state_post, measurement, measurement );

/* adjust Kalman filter state */

/* Kk=P'k鈥 T鈥?H鈥 'k鈥 T+R)-1

xk=x'k+Kk鈥?zk-H鈥 'k)

Pk=(I-Kk鈥 )鈥 'k */

cvKalmanCorrect( cvkalman, measurement );

float measured_value_x = measurement->data.fl[0];

float measured_value_y = measurement->data.fl[2];

const CvMat* prediction = cvKalmanPredict( cvkalman, 0 );

float predict_value_x = prediction->data.fl[0];

float predict_value_y = prediction->data.fl[2];

return(cvPoint2D32f(predict_value_x,predict_value_y));

}

void kalman::init_kalman(int x,int xv,int y,int yv)

{

state->data.fl[0]=x;

state->data.fl[1]=xv;

state->data.fl[2]=y;

state->data.fl[3]=yv;

cvkalman->state_post->data.fl[0]=x;

cvkalman->state_post->data.fl[1]=xv;

cvkalman->state_post->data.fl[2]=y;

cvkalman->state_post->data.fl[3]=yv;

}

********************************************************************* **************************************

卡尔曼滤波算法与matlab实现

一个应用实例详解卡尔曼滤波及其算法实现 标签:算法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

扩展卡尔曼滤波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;

卡尔曼滤波入门简介及其算法MATLAB实现代码

卡尔曼滤波入门: 卡尔曼滤波是用来进行数据滤波用的,就是把含噪声的数据进行处理之后得出相对真值。卡尔曼滤波也可进行系统辨识。 卡尔曼滤波是一种基于统计学理论的算法,可以用来对含噪声数据进行在线处理,对噪声有特殊要求,也可以通过状态变量的增广形式实现系统辨识。 用上一个状态和当前状态的测量值来估计当前状态,这是因为上一个状态估计此时状态时会有误差,而测量的当前状态时也有一个测量误差,所以要根据这两个误差重新估计一个最接近真实状态的值。 信号处理的实际问题,常常是要解决在噪声中提取信号的问题,因此,我们需要寻找一种所谓有最佳线性过滤特性的滤波器。这种滤波器当信号与噪声同时输入时,在输出端能将信号尽可能精确地重现出来,而噪声却受到最大抑制。 维纳(Wiener)滤波与卡尔曼(Kalman)滤波就是用来解决这样一类从噪声中提取信号问题的一种过滤(或滤波)方法。 (1)过滤或滤波 - 从当前的和过去的观察值x(n),x(n-1),x(n-2),…估计当前的信号值称为过滤或滤波; (2)预测或外推 - 从过去的观察值,估计当前的或将来的信号值称为预测或外推; (3)平滑或内插 - 从过去的观察值,估计过去的信号值称为平滑或内插; 因此,维纳过滤与卡尔曼过滤又常常被称为最佳线性过滤与预测或线性最优估计。这里所谓“最佳”与“最优”是以最小均方误差为准则的。 维纳过滤与卡尔曼过滤都是解决最佳线性过滤和预测问题,并且都是以均方误差最小为准则的。因此在平稳条件下,它们所得到的稳态结果是一致的。然而,它们解决的方法有很大区别。 维纳过滤是根据全部过去的和当前的观察数据来估计信号的当前值,它的解是以均方误差最小条件下所得到的系统的传递函数H(z)或单位样本响应h(n)的形式给出的,因此更常称这种系统为最佳线性过滤器或滤波器。 而卡尔曼过滤是用前一个估计值和最近一个观察数据(它不需要全部过去的观察数据)来估计信号的当前值,它是用状态方程和递推的方法进行估计的,它的解是以估计值(常常是状态变量值)形式给出的。因此更常称这种系统为线性最优估计器或滤波器。 维纳滤波器只适用于平稳随机过程,而卡尔曼滤波器却没有这个限制。维纳过滤中信号和噪声是用相关函数表示的,因此设计维纳滤波器要求已知信号和噪声的相关函数。 卡尔曼过滤中信号和噪声是状态方程和量测方程表示的,因此设计卡尔曼滤波器要求已知状态方程和量测方程(当然,相关函数与状态方程和量测方程之间会存在一定的关系。卡尔曼过滤方法看来似乎比维纳过滤方法优越,它用递推法计算,不需要知道全部过去的数据,从而运用计算机计算方便,而且它可用于平稳和不平稳的随机过程(信号),非时变和时变的系统。 但从发展历史上来看维纳过滤的思想是40年代初提出来的,1949年正式以书的形式出版。卡尔曼过滤到60年代初才提出来,它是在维纳过滤的基础上发展起来的,虽然如上所述它比维纳过滤方法有不少优越的地方,但是最佳线性过滤问题是由维纳过滤首先解决的,维纳过滤的物理概念比较清楚,也可以认为卡尔曼滤波仅仅是对最佳线性过滤问题提出的一种新的算法。 卡尔曼滤波在数学上是一种统计估算方法,通过处理一系列带有误差的实际量测数据而得到的物理参数的最佳估算。例如在气象应用上,根据滤波的基本思想,利用前一时刻预报误差的反馈信息及时修正预报方程,以提高下一时刻预报精度。作温度预报一般只需要连续两个月的资料即可建立方程和递推关系。

几种卡尔曼滤波算法理论

自适应卡尔曼滤波 卡尔曼滤波发散的原因 如果卡尔曼滤波是稳定的,随着滤波的推进,卡尔曼滤波估计的精度应该越来越高,滤波误差方差阵也应趋于稳定值或有界值。但在实际应用中,随着量测值数目的增加,由于估计误差的均值和估计误差协方差可能越来越大,使滤波逐渐失去准确估计的作用,这种现象称为卡尔曼滤波发散。 引起滤波器发散的主要原因有两点: (1)描述系统动力学特性的数学模型和噪声估计模型不准确,不能直接真实地反映物理过程,使得模型与获得的量测值不匹配而导致滤波发散。这种由于模型建立过于粗糙或失真所引起的发散称为滤波发散。 (2)由于卡尔曼滤波是递推过程,随着滤波步数的增加,舍入误差将逐渐积累。如果计算机字长不够长,这种积累误差很有可能使估计误差方差阵失去非负定性甚至失去对称性,使滤波增益矩阵逐渐失去合适的加权作用而导致发散。这种由于计算舍入误差所引起的发散称为计算发散。 针对上述卡尔曼滤波发散的原因,目前已经出现了几种有效抑制滤波发散的方法,常用的有衰减记忆滤波、限定记忆滤波、扩充状态滤波、有限下界滤波、平方根滤波、和自适应滤波等。这些方法本质上都是以牺牲滤波器的最优性为代价来抑制滤波发散,也就是说,多数都是次优滤波方法。 自适应滤波 在很多实际系统中,系统过程噪声方差矩阵Q和量测误差方差阵R事先是不知道的,有时甚至连状态转移矩阵 或量测矩阵H也不能确切建立。如果所建立的模型与实际模型不符可能回引起滤波发散。自适应滤波就是这样一种具有抑制滤波发散作用的滤波方法。在滤波过程中,自适应滤波一方面利用量测值修正预测值,同时也对未知的或不确切的系统模型参数和噪声统计参数进行估计修正。自适应滤波的方法很多,包括贝叶斯法、极大似然法、相关法与协方差匹配法,其中最基本也是最重要的是相关法,而相关法可分为输出相关法和新息相关法。

卡尔曼滤波器及其简matlab仿真

卡尔曼滤波器及其简matlab仿真

卡尔曼滤波器及其简matlab仿真 一、卡尔曼滤波的起源 谈到信号的分析与处理,就离不开滤波两个字。通常,信号的频谱处于有限的频率范围内,而噪声的频谱则散布在很广的频率范围内,为了消除噪声,可以进行频域滤波。但在许多应用场合,需要直接进行时域滤波,从带噪声的信号中提取有用信号。虽然这样的过程其实也算是对信号的滤波,但其所依据的理论,即针对随机信号的估计理论,是自成体系的。人们对于随机信号干扰下的有用信号不能“确知”,只能“估计”。为了“估计”,要事先确定某种准则以评定估计的好坏程度。 1960年卡尔曼发表了用递归方法解决离散数据线性滤波问题的论文A New Approach to Linear Filtering and Prediction Problems (线性滤波与预测问题的新方法),在这篇文章里一种克服了维纳滤波缺点的新方法被提出来,这就是我们今天称之为卡尔曼滤波的方法。卡尔曼滤波应用广泛且功能强大,它可以估计信号的过去和当前状态甚至能估计将来的状态即使并不知道模型的确切性质。 其基本思想是以最小均方误差为最佳估计准则,采用信号与噪声的状态空间模型利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计,求出当前时刻的估计值。算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。 对于解决很大部分的问题,它是最优,效率最高甚至是最有用的。它的广泛应用已经超过30年,包括机器人导航,控制,传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理,例如头脸识别,图像分割,图像边缘检测等等。 卡尔曼滤波不要求保存过去的测量数据,当新的数据到来时,根据新的数据和前一时刻的储值的估计,借助于系统本身的状态转移方程,按照一套递推公式,即可算出新的估值。卡尔曼递推算法大大减少了滤波装置的存储量和计算量,并且突破了平稳随机过程的限制,使卡尔曼滤波器适用于对时变信号的实时处理。

卡尔曼滤波算法总结

卡尔曼滤波算法总结-标准化文件发布号:(9556-EUATWK-MWUB-WUNN-INNUL-DDQTY-KII

2015.12.12 void 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)(1|1)() X k k AX k k Bu k -=--+(1)先验估计 (|1)(1|1)'P k k AP k k A Q -=--+(2)协方差矩阵的预测 ()(|1)'/(|1)')Kg k P k k H HP k k H R =--+(3)计算卡尔曼增益 (|)(|1)()(()(|1))X k k X k k Kg k Z k HX k k =-+--(4)进行修正 5个式子比较抽象,现在直接用实例来说: 一、卡尔曼滤波第一个式子 对于角度来说,我们认为此时的角度可以近似认为是上一时刻的角度值加上上一时刻陀螺仪测得的角加速度值乘以时间,因为d dt θω=?,角度微分等于时间的微分乘以角速度。但是陀螺仪有个静态漂移(而且还是变化的),静态漂移就是静止了没有角速度然后陀螺仪也会输出一个值,这个值肯定是没有意义的,计算时要把它减去。 由此我们得到了当前角度的预测值Angle Angle=Angle+(Gyro - Q_bias) * dt; 其中等号左边Angle 为此时的角度,等号右边Angle 为上一时刻的角度,Gyro 为陀螺仪测的角速度的值,dt 是两次滤波之间的时间间隔,我们的运行周期是4ms 或者6ms 。 同时 Q_bias 也是一个变化的量。 但是就预测来说认为现在的漂移跟上一时刻是相同的,即 Q_bias=Q_bias 将上面两个式子写成矩阵的形式 1_0 1_0 Angle dt Angle dt Q bias Q bia o s Gyr -= + 得到上式,这个式子对应于卡尔曼滤波的第一个式子 (|1)(1|1)() X k k AX k k Bu k -=--+ (|)(|1) P k k I Kg k H P k k =--(())(5)更新协方差阵

卡尔曼滤波器及其简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.sodocs.net/doc/a712334727.html,/~welch/media/pdf/Kalman1960.pdf。 简单来说,卡尔曼滤波器是一个“optimal recursive data processing algorithm(最优化自回归数据处理算法)”。对于解决很大部分的问题,他是最优,效率最高甚至是最有用的。他的广泛应用已经超过30年,包括机器人导航,控制,传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理,例如头脸识别,图像分割,图像边缘检测等等。 2.卡尔曼滤波器的介绍 (Introduction to the Kalman Filter) 为了可以更加容易的理解卡尔曼滤波器,这里会应用形象的描述方法来讲解,而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是,他的5条公式是其核心内容。结合现代的计算机,其实卡尔曼的程序相当的简单,只要你理解了他的那5条公式。 在介绍他的5条公式之前,先让我们来根据下面的例子一步一步的探索。 假设我们要研究的对象是一个房间的温度。根据你的经验判断,这个房间的温度是恒定的,也就

(完整word版)扩展卡尔曼滤波算法的matlab程序

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;

卡尔曼滤波两例题含matlab程序汇总

设高度的测量误差是均值为0、方差为1的高斯白噪声随机序列,该物体的初始高度0h 和速度0V 也是高斯分布的随机变量,且0000019001000,var 10/02Eh h m P EV m s V ???????? ===? ??????? ???? ????。试求该物体高度和速度随时间变化的最优估计。(2/80.9s m g =) 解: 1. 令()()()h k X k v k ?? =? ??? t=1 R (k )=1 Q(k)=0 根据离散时间卡尔曼滤波公式,则有: (1)(1,)()()X k k k X k U k φ+=++ (1)(1)(1)(1)Y k H k X k V k +=++++ (1,)k k φ+= 11t -?? ??? ? ()U k = 20.5gt gt ??-???? (1)H k +=[]10 滤波初值:^ 1900(0|0)(0)10X EX ?? ==???? 0100(0|0)var[(0)]2P X P ?? ===? ??? 一步预测:^^ (1|)(1,)(|)()X k k k k X k k U k φ+=++ (1|)(1,)(|)(1,)T P k k k k P k k k k φφ+=++ 滤波增益:1 (1)(1|)(1)[(1)(1|)(1)(1)]T T K k P k k H k H k P k k H k R k -+=+++++++ 滤波计算:^ ^ ^ (1|1)(1|)(1)[(1)(1)(1|)]X k k X k k K k Y k H k X k k ++=++++-++ (1|1)[(1)(1)](1|)P k k I K k H k P k k ++=-+++ 2. 实验结果

matlab对卡尔曼滤波的仿真实现

MATLAB 对卡尔曼滤波器的仿真实现 刘丹,朱毅,刘冰 武汉理工大学信息工程学院,武汉(430070) E-mail :liudan_ina@https://www.sodocs.net/doc/a712334727.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()(?

基于扩展卡尔曼滤波算法的matlab程序

扩展卡尔曼滤波原理: 在原有卡尔曼滤波的基础上,为了解决多目标值的跟踪与估计,形成了扩展卡尔曼滤波。起matlab主要程序如下: 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));

维纳最速下降法滤波器卡尔曼滤波器设计及Matlab仿真

信息融合大作业 ——维纳最速下降法滤波器,卡尔曼滤波器设计及Matlab仿真 1.滤波问题浅谈 估计器或滤波器这一术语通常用来称呼一个系统,设计这样的系统是为了从含有噪声的数据中提取人们感兴趣的,接近规定质量的信息。由于这样一个宽目标,估计理论应用于诸如通信、雷达、声纳、导航、地震学、生物医学工程、 金融工程等众多不同的领域。例如,考虑一个数字通信系统,其基本形式由发

射机、信道和接收机连接组成。发射机的作用是把数字源(例如计算机)产生的0、1符号序列组成的消息信号变换成为适合于信道上传送的波形。而由于符号间干扰和噪声的存在,信道输出端收到的信号是含有噪声的或失真的发送信号。接收机的作用是,操作接收信号并把原消息信号的一个可靠估值传递给系统输出端的某个用户。随着通信系统复杂度的提高,对原消息信号的还原成为通信系统中最为重要的环节,而噪声是接收端需要排除的最主要的干扰,人们也设计出了针对各种不同条件应用的滤波器,其中最速下降算法是一种古老的最优化技术,而卡尔曼滤波器随着应用条件的精简成为了普适性的高效滤波器。2.维纳最速下降算法滤波器 2.1 最速下降算法的基本思想 考虑一个代价函数,它是某个未知向量的连续可微分函数。函数 将的元素映射为实数。这里,我们要寻找一个最优解。使它满足如下条件 (2.1) 这也是无约束最优化的数学表示。 特别适合于自适应滤波的一类无约束最优化算法基于局部迭代下降的算法: 从某一初始猜想出发,产生一系列权向量,使得代价函数在算法的每一次迭代都是下降的,即 其中是权向量的过去值,而是其更新值。 我们希望算法最终收敛到最优值。迭代下降的一种简单形式是最速下降法,该方法是沿最速下降方向连续调整权向量。为方便起见,我们将梯度向量表示为

卡尔曼滤波算法及MATLAB实现

基于matlab的卡尔曼信号滤波设计 卡尔曼滤波的基本思想是:以最小均方误差为最佳估计准则,采用信号与噪声的状态空间模型,利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计,求出当前时刻的估计值,算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。 语音信号在较长时间内是非平稳的,但在较短的时间内的一阶统计量和二阶统计量近似为常量,因此语音信号在相对较短的时间内可以看成白噪声激励以线性时不变系统得到的稳态输出。假定语音信号可看成由一AR模型产生: 时间更新方程: 测量更新方程: K(t)为卡尔曼增益,其计算公式为: 其中 、分别为过程模型噪声协方差和测量模型噪声协方差,测量协方差可以通过观测得到, 则较难确定,在本实验中则通过与两者比较得到。 由于语音信号短时平稳,因此在进行卡尔曼滤波之前对信号进行分帧加窗操作,在滤波之后对处理得到的信号进行合帧,这里选取帧长为256,而帧重叠个数为128; 下图为原声音信号与加噪声后的信号以及声音信号与经卡尔曼滤波处理后的信号:

原声音信号与加噪声后的信号 原声音信号与经卡尔曼滤波处理后的信号 MATLAB程序实现如下: %%%%%%%%%%%%%%%%%基于LPC全极点模型的最大后验概率估计法,采用卡尔曼滤波%%%%%%%%%%%%%% clear; clc; %%%%%%%%%%%%%%%%%%%%%%%%%%%加载声音数据%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% loadvoice.mat y=m1(2,:); x=y+0.08*randn(1,length(y)); %%%%%%%%%%%%%%%原声音信号和加噪声后的信号%%%%%%%%%%%%%%% figure(1); subplot(211);plot(m1(1,:),m1(2,:));xlabel('时间');ylabel('幅度');title('原声音信号'); subplot(212);plot(m1(1,:),x);xlabel('时间');ylabel('幅度');title('加噪声后的信号');

卡尔曼滤波算法总结

2015.12.12 void 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)(1|1)()X k k AX k k Bu k -=--+(1)先验估计 (|1)(1|1)'P k k AP k k A Q -=--+(2)协方差矩阵的预测 ()(|1)'/(|1)') Kg k P k k H HP k k H R =--+(3)计算卡尔曼增益 (|)(|1)()(()(|1)) X k k X k k Kg k Z k HX k k =-+--(4)进行修正 5个式子比较抽象,现在直接用实例来说: 一、卡尔曼滤波第一个式子 对于角度来说,我们认为此时的角度可以近似认为是上一时刻的角度值加上上一时刻陀螺仪测得的角加速度值乘以时间,因为d dt θω=?,角度微分等于时间的微分乘以角速度。但是陀螺仪有个静态漂移(而且还是变化的),静态漂移就是静止了没有角速度然后陀螺仪也会输出一个值,这个值肯定是没有意义的,计算时要把它减去。 由此我们得到了当前角度的预测值Angle Angle=Angle+(Gyro - Q_bias) * dt; 其中等号左边Angle 为此时的角度,等号右边Angle 为上一时刻的角度,Gyro 为陀螺仪测的角速度的值,dt 是两次滤波之间的时间间隔,我们的运行周期是4ms 或者6ms 。 同时 Q_bias 也是一个变化的量。 但是就预测来说认为现在的漂移跟上一时刻是相同的,即 Q_bias=Q_bias 将上面两个式子写成矩阵的形式 1_01_0 Angle dt Angle dt Q bias Q bia o s Gyr -=+ 得到上式,这个式子对应于卡尔曼滤波的第一个式子 (|1)(1|1)()X k k AX k k Bu k -=--+ ()|1X k k -为2维列向量 _Angle Q bias ,A 为2维方阵101dt -,()|-11X k k -为2维列向量_Angle Q bias ,B 为2维列向量0dt , ()u k 为Gyro (|)(|1) P k k I Kg k H P k k =--(())(5)更新协方差阵

完整word版Kalman滤波MATLAB综合实验报告

《数学实验》综合实验报告实验名称综合实验(Kalman滤波) 2016年 5月

一、【实验目的】 明白滤波计算流程 能够调用相关函数进行数据处理 使用循环函数和二维曲线画图 有效的构建仿真模型,产生模拟数据 二、【实验原理分析】 卡尔曼滤波器是一个“optimal recursive data processing algorithm(最优化自回归数据处理算法)”。对于解决很大部分的问题,它是最优,效率最高甚至是最有用的。它的广泛应用已经超过30年,包括机器人导航,控制,传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近来更被应用于计算机图像处理,例如头脸识别,图像分割,图像边缘检测等等。 设系统可用一个线性随机微分方程来描述: X(k)=A X(k-1)+B U(k)+W(k) 再加上系统的测量值:Z(k)=H X(k)+V(k) 上两式子中,X(k)是k时刻的系统状态,U(k)是k时刻对系统的控制量。A和B是系统参数,对于多模型系统,他们为矩阵。Z(k)是k时刻的测量值,H是测量系统的参数,对于多测量系统,H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声,他们的协方差分别是Q,R(这里假设他们不随系统状态变化而变化)。 对于满足上面的条件(线性随机微分系统,过程和测量都是高斯白噪声),卡尔曼滤波器是最优的信息处理器。首先要利用系统的过程模型,来预测下一状态的系统。假设现在的系统状态是k,根据系统的模型,可以基于系统的上一状态而预测出现在状态: X(k|k-1)=A X(k-1|k-1)+B U(k) (1) 式(1)中,X(k|k-1)是利用上一状态预测的结果,X(k-1|k-1)是上一状态最优的结果,U(k)为现在状态的控制量,如果没有控制量,它可以为0。到现在为止,我们的系统结果已经更新了,可是,对应于X(k|k-1)的协方差还没更新。我们用P表示协方差: P(k|k-1)=A P(k-1|k-1) A'+Q (2) 式(2)中,P(k|k-1)是X(k|k-1)对应的协方差,P(k-1|k-1)是X(k-1|k-1)对应的协方差,A'表示A的转置矩阵,Q是系统过程的协方差。式子1,2就是卡尔曼滤波器5个公式当中的前两个,也就是对系统的预测。 现在我们有了现在状态的预测结果,然后我们再收集现在状态的测量值。结合预测值和测量值,我们 2 可以得到现在状态(k)的最优化估算值X(k|k): X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) (3) 其中Kg为卡尔曼增益(Kalman Gain): Kg(k)= P(k|k-1) H' / (H P(k|k-1) H' + R) (4) 到现在为止,我们已经得到了k状态下最优的估算值X(k|k)。但是为了要令卡尔曼滤波器不断的运行下去直到系统过程结束,我们还要更新k状态下X(k|k)的协方差:

相关主题