卡尔曼滤波及代码matlab C C++.docx
- 文档编号:18606416
- 上传时间:2023-08-20
- 格式:DOCX
- 页数:17
- 大小:18.42KB
卡尔曼滤波及代码matlab C C++.docx
《卡尔曼滤波及代码matlab C C++.docx》由会员分享,可在线阅读,更多相关《卡尔曼滤波及代码matlab C C++.docx(17页珍藏版)》请在冰点文库上搜索。
卡尔曼滤波及代码matlabCC++
卡尔曼滤波器(KalmanFilter)是一个最优化自回归数据处理算法(optimalrecursivedataprocessingalgorithm)。
对于解决很大部分的问题,他是最优,效率最高甚至是最有用的。
他的广泛应用已经超过30年,包括机器人导航,控制,传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。
近年来更被应用于计算机图像处理,例如头脸识别,图像分割,图像边缘检测等等。
最佳线性滤波理论起源于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)
计算预估计协方差矩阵
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)'
计算卡尔曼增益矩阵
K(k)=C(k)^×H(k)'×[H(k)×C(k)^×H(k)'+R(k)]^(-1)
R(k)=N(k)×N(k)'
更新估计
X(k)~=X(k)^+K(k)×[Y(k)-H(k)×X(k)^]
计算更新后估计协防差矩阵
C(k)~=[I-K(k)×H(k)]×C(k)^×[I-K(k)×H(k)]'+K(k)×R(k)×K(k)'
X(k+1)=X(k)~
C(k+1)=C(k)~
重复以上步骤
**********************************************
Matlab实现代码[转]
*********************************************************************************************************************************
%%%%ConstantVelocityModelKalmanFilterSimulation%%%%
%==========================================================================
clearall;closeall;clc;
%%Initialcondition
ts=1;%Samplingtime
t=[0:
ts:
100];
T=length(t);
%%Initialstate
x=[040020]';
x_hat=[0000]';
%%Processnoisecovariance
q=5
Q=q*eye
(2);
%%Measurementnoisecovariance
r=5
R=r*eye
(2);
%%Processandmeasurementnoise
w=sqrt(Q)*randn(2,T);%Processnoise
v=sqrt(R)*randn(2,T);%Measurementnoise
%%Estimateerrorcovarianceinitialization
p=5;
P(:
:
1)=p*eye(4);
%==========================================================================
%%Continuous-timestatespacemodel
%{
x_dot(t)=Ax(t)+Bu(t)
z(t)=Cx(t)+Dn(t)
%}
A=[0100;
0000;
0001;
0000];
B=[00;
10;
00;
01];
C=[1000;
0010];
D=[10;
01];
%%Discrete-timestatespacemodel
%{
x(k+1)=Fx(k)+Gw(k)
z(k)=Hx(k)+Iv(k)
Continuoustodiscreteformbyzoh
%}
sysc=ss(A,B,C,D);
sysd=c2d(sysc,ts,'zoh');
[FGHI]=ssdata(sysd);
%%Practicestateoftarget
fori=1:
T-1
x(:
i+1)=F*x(:
i);
end
x=x+G*w;%Statevariablewithnoise
z=H*x+I*v;%Measurementvaluewithnoise
%==========================================================================
%%%KalmanFilter
fori=1:
T-1
%%Predictionphase
x_hat(:
i+1)=F*x_hat(:
i);
%Stateestimatepredict
P(:
:
i+1)=F*P(:
:
i)*F'+G*Q*G';
%Trackingerrorcovariancepredict
P_predicted(:
:
i+1)=P(:
:
i+1);
%%Kalmangain
K=P(:
:
i+1)*H'*inv(H*P(:
:
i+1)*H'+R);
%%Updatastep
x_hat(:
i+1)=x_hat(:
i+1)+K*(z(:
i+1)-H*x_hat(:
i+1));
%Stateestimateupdate
P(:
:
i+1)=P(:
:
i+1)-K*H*P(:
:
i+1);
%Trackingerrorcovarianceupdate
P_updated(:
:
i+1)=P(:
:
i+1);
end
%==========================================================================
%%Estimateerror
x_error=x-x_hat;
%%Graph1practicalandtrackingposition
figure
(1)
plot(x(1,:
),x(3,:
),'r');
holdon;
plot(x_hat(1,:
),x_hat(3,:
),'g.');
title('2DTargetPosition')
legend('PracticalPosition','TrackingPosition')
xlabel('Xaxis[m]')
ylabel('Yaxis[m]')
holdoff;
%%Graph2
figure
(2)
plot(t,x(1,:
)),gridon;
holdon;
plot(t,x_hat(1,:
),'r'),gridon;
title('PracticalandTrackingPositiononXaxis')
legend('PracticalPosition','TrackingPosition')
xlabel('Time[sec]')
ylabel('Position[m]')
holdoff;
%%Graph3
figure(3)
plot(t,x_error(1,:
)),gridon;
title('PositionErroronXaxis')
xlabel('Time[sec]')
ylabel('PositionRMSE[m]')
holdoff;
%%Graph4
figure(4)
plot(t,x(2,:
)),gridon;
holdon;
plot(t,x_hat(2,:
),'r'),gridon;
title('PracticalandTrackingVelocityonXaxis')
legend('PracticalVelocity','TrackingVelocity')
xlabel('Time[sec]')
ylabel('Velocity[m/sec]')
holdoff;
%%Graph5
figure(5)
plot(t,x_error(2,:
)),gridon;
title('VelocityErroronXaxis')
xlabel('Time[sec]')
ylabel('VelocityRMSE[m/sec]')
holdoff;
%==========================================================================
*********************************************************************************************************************************
***********************************************
c语言实现代码 [转]
-----------------------------------------------------------------------------------------------------------------------------
#include"stdlib.h"
#include"rinv.c"
intlman(n,m,k,f,q,r,h,y,x,p,g)
intn,m,k;
doublef[],q[],r[],h[],y[],x[],p[],g[];
{inti,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: interfaceforthekalmanclass. // ////////////////////////////////////////////////////////////////////// #if! defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLUDED_) #defineAFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLUDED_ #if_MSC_VER>1000 #pragmaonce #endif//_MSC_VER>1000 #include #include"cv.h" classkalman { public: voidinit_kalman(intx,intxv,inty,intyv); CvKalman*cvkalman; CvMat*state; CvMat*process_noise; CvMat*measurement; constCvMat*prediction; CvPoint2D32fget_predict(floatx,floaty); kalman(intx=0,intxv=0,inty=0,intyv=0); //virtual~kalman(); }; #endif//! defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLUDED_) ============================kalman.cpp================================ #include"kalman.h" #include /*testerdeprintertouteslesvaleursdesvecteurs*/ /*testerdechangerlesmatricesdunoises*/ /*replacestatebycvkalman->state_post*/ CvRandStaterng; constdoubleT=0.1; kalman: : kalman(intx,intxv,inty,intyv) { cvkalman=cvCreateKalman(4,4,0); state=cvCreateMat(4,1,CV_32FC1); process_noise=cvCreateMat(4,1,CV_32FC1); measurement=cvCreateMat(4,1,CV_32FC1); intcode=-1; /*creatematrixdata*/ constfloatA[]={ 1,T,0,0, 0,1,0,0, 0,0,1,T, 0,0,0,1 }; constfloatH[]={ 1,0,0,0, 0,0,0,0, 0,0,1,0, 0,0,0,0 }; constfloatP[]={ 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) }; constfloatQ[]={ 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 }; constfloatR[]={ 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)); /*chooseinitialstate*/ 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->
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 卡尔曼滤波及代码matlab C+ 卡尔 滤波 代码 matlab