【信号去噪】基于卡尔曼滤波和维纳滤波实现信号去噪附matlab代码 上传.zip

clear; clc; close all N=100; v=randn(1,N); a1=-1.352;a2=1.338;a3=-0.662;a4=0.240; x(1)=v(1); x(2)=-a1*x(1)+v(2); x(3)=-a1*x(2)-a2*x(1)+v(3); x(4)=-a1*x(3)-a2*x(2)-a3*x(1)+v(4); for n=1:N-4; x(n+4)=-a1*x(n+3)-a2*x(n+2)-a3*x(n+1)-a4*x(n)+v(n+4); %产生真实数据 end v2=randn(1,N);%观测噪声 z=x+v2;%产生观测数据 %卡尔曼滤波 %赋初值 A=[-a1 -a2 -a3 -a4;1 0 0 0;0 1 0 0;0 0 1 0];%状态转移矩阵 H=[1 0 0 0];%观测矩阵 Q=[1 0 0 0;0 0 0 0;0 0 0 0;0 0 0 0];%状态噪声方差阵 R=1;%观测噪声方差阵 X(:,1)=[z(4);z(3);z(2);z(1)]; p(:,:,1)=[10 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];%一步预测误差方差阵 %开始滤波 for k=2:N p1(:,:,k)=A*p(:,:,k-1)*A'+Q; %p1(:,:,k)即是一步预测误差的自相关矩阵，它是4*4的矩阵，取不同的k值就构成了一个三维矩阵 K(:,k)=p1(:,:,k)*H'/(H*p1(:,:,k)*H'+R); %K(:,:,k)是增益矩阵,对于固定的k值它是4*1矩阵，取不同的k值就是三维矩阵 X(:,k)=A*X(:,k-1)+K(:,k)*[z(k)-H*A*X(:,k-1)]; % X(:,k)是估计值,4*1矩阵 p(:,:,k)=p1(:,:,k)-K(:,k)*H*p1(:,:,k); %p(:,:,k)是估计误差的自相关矩阵，4*4矩阵的三维矩阵 end %结束一次滤波 %估计值与真实值之差 error_px=X(1,:)-x;%x距离误差 %绘图 t=1:N; figure(1); plot(t,x,'k-',t,z,'g:',t,X(1,:),'r-.'); legend('真实轨迹','观测样本','估计轨迹'); figure(2) plot(error_px); legend('估计误差');