基本的卡尔曼滤波和自适应滤波【Matlab语音处理】.zip

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %矢量卡尔曼滤波处理 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%建立信号模型和测量模型%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% T=0.1;%数据采样时间 t=0:T:1000-T; N=length(t); vr=0.1;%目标加速度的方差 dv=0.5;%速度测量误差的方差 dr=0.5;%距离测量误差的方差 v_jia=vr*randn(1,N);%目标的瞬时加速度 y=zeros(1,N);%目标的位置 v=zeros(1,N);%目标的速度 y(1)=0;%目标初始位置 v(1)=1;%目标初始运动速度 for i=2:N v(i)=v(i-1)+T*v_jia(i); end for i=2:N y(i)=y(i-1)+T*v(i); end yx=y+dr*randn(1,N);%测量的距离值 vx=v+dv*randn(1,N);%测量的速度值 S=[yx;vx]; S_guji=zeros(2,N);%滤波后的值 S_guji(:,1)=[0,10]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%对测量后的结果进行卡尔曼滤波处理%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% A=[1,T;0,1];%信号的模型系数矩阵 C=[1,0;0,1];%测量方程的系数矩阵 R=[dr,0;0,dv];%测量误差的自相关矩阵 CW=[dr,1/T*dr;1/T*dr,2/T^2*dr+T^2*vr];%预测误差矩阵 Q=[0,0;0,T^2*vr]; for i=2:N P=A*CW*A.'+Q; G=P*C.'*inv(C*P*C.'+R); CW=[eye(2,2)-C*G]*P; S_guji(:,i)=A*S_guji(:,i-1)+G*(S(:,i)-A*C*S_guji(:,i-1)); end figure(1); plot(t,y,'r-',t,S_guji(1,:),'g-',t,S(1,:),'b.'); title('位置滤波'); legend('位置真实值','位置估计值','位置测量值'); figure(2); plot(t,v,'r-',t,S_guji(2,:),'g-',t,S(2,:),'b.'); title('速度滤波'); legend('速度真实值','速度估计值','速度测量值'); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%