✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。
🍎个人主页：Matlab科研工作室
🍊个人信条：格物致知。
更多Matlab仿真内容点击👇
⛄ 内容介绍
针对传统的维纳滤波器实现过程中存在的运算量过于复杂且只能处理平稳和一维信号的缺点,结合卡尔曼滤波相关理论,使用MATLAB软件,设计算法,实现了可以用来处理多维和非平稳的随机信号的卡尔曼滤波器.测试结果表明,卡尔曼滤波器在语音去噪,目标追踪中效果更为显著,功能更加强大.该研究为信号处理技术的进一步发展提供了价值.
⛄ 部分代码
clear;
clf;
close all;
rng(3);
n = 50;
a = .7;
tau = 20; %missing entries tau1 to tau2
tau1 = 20;
tau2 = 29;
yplot = [0:51];
%generate errors in X
v = randn(n,1);
Q = 1; %error variance. NOTE: need to modify above function if changed
vecX(:,1) = v(1);
for i = 2:n
vecX(:,i) = a * vecX(:,i - 1) + v(i);
end
for i = 1:n
H(:, i) = 1;
end
for i = tau1:tau2
H(:, i) = 0;
end
H = permute(H, [2 1]);
for i = 1:n
vecY(i) = H(i, 1)' * vecX(:, i);
end
% Xhat(:,:,1) = 0';
% P(:,:,1) = 1; %note: error variance = 1 here
% for i = 2:n + 1
% if i ~= tau + 1
% partK = P(:, :, i - 1)*H(:,:,i - 1)*inv( H(:,:,i - 1)' * P(:, :, i - 1) *H(:,:,i - 1));
% Xhatt(:,:,i - 1) = Xhat(:, :, i - 1) + F(:, :, i - 1)* partK * (vecY(i - 1) - H(:, :, i - 1)' * Xhat(:,:, i - 1));
% Xhat(:, :,i) = F(:, :, i - 1)*Xhatt(:, :, i - 1);
% Pt(:,:,i - 1) = P(:,:,i - 1) - partK * H(:,:,i - 1)' *P(:,:,i - 1);
% P(:,:,i) = F(:,:,i - 1)* Pt(:,:,i - 1) * F(:, :, i - 1)' + [1 0; 0 0];
% else
% Xhatt(:,:,i - 1) = Xhat(:, :, i - 1);
% Xhat(:, :,i) = F(:, :, i - 1)*Xhat(:, :, i - 1);
% Pt(:,:, i - 1) = P(:, :, i - 1);
% P(:,:,i) = F(:,:,i - 1)* P(:,:,i - 1) * F(:,:, i - 1)' + [1 0; 0 0];
% end
% end
Xhat = vecY;
for i = tau1:tau2
Xhat(i) = a * Xhat(i - 1);
end
%plot(Xhat, 'color','black');
figure('Name','Blue: state. Red: missing data. Green: filter. Black: smoother.')
hold on
plot(vecX(1:tau1 - 1),'color','blue')
plot(yplot(tau2 + 2:n + 1),vecX(tau2 + 1:n),'color','blue')
plot(yplot(tau1 + 1:tau2 + 1),vecX(1,tau1:tau2),'color','red');
plot(yplot(tau1 + 1:tau2 + 1),Xhat(1, tau1:tau2),'color','green')
for i = 1:tau1 - 1
P(i) = Q;
Pt(i) = 0;
end
for i = tau1:tau2
P(i) = a^2 * Pt(i - 1) + Q;
Pt(i) = P(i);
end
for i = tau2 + 1:n
P(i) = Q;
Pt(i) = 0;
end
P(tau2 + 1) = a^2 * Pt(tau2) + Q; %note: above loop just fills a 0 for P(tau2 + 1). This is correct value.
for i = 1:n
J(i) = 0;
end
for i = tau1:tau2
J(i) = a*Pt(i)*(1/P(i + 1));
end
Xhat2 = Xhat;
Xhat2(tau2 + 1) = a*Xhat(tau2); %prediction of next available point from last missing point
Xs = Xhat;
for i = tau2:-1:tau1
Xs(i) = Xhat2(i) + J(i)*(Xs(i + 1) - Xhat2(i + 1));
end
plot(yplot(tau1 + 1:tau2 + 1),Xs(tau1:tau2),'color','black');
% for i = tau:n
% J(:,:,i) = Pt(:,:,i) *F(:,:,i)' * inv(P(:,:,i + 1));
% end
%
% S(:,:,n + 1) = Xhat(:,:,n + 1);
% for i = 1:n
% S(:,:,n + 1 - i) = Xhatt(:,:,n + 1 - i) + J(:,:,n + 1 - i) * (S(:,:,n + 2 - i) - Xhat(:,:,n + 2 - i));
% end
%
% %plot(S(1,:));
%
%
% r(:,n + 1) = [0 0]';
% for i = n:-1:tau + 1
% r(:,i) = H(:,:,i)*inv(H(:,:,i)' * P(:,:,i + 1) * H(:,:,i)) ...
% + (F(:,:,i) - F(:,:,i)*P(:,:,i + 1)*H(:,:,i)*inv(H(:,:,i)' * P(:,:,i + 1) * H(:,:,i))*H(:,:,i)')*r(:,i + 1);
% end
% r(:,tau) = F(:,:,tau) * r(:,tau + 1);
% for i = tau - 1:-1:1
% r(:,i) = H(:,:,i)*inv(H(:,:,i)' * P(:,:,i + 1) * H(:,:,i)) ...
% + (F(:,:,i) - F(:,:,i)*P(:,:,i + 1)*H(:,:,i)*inv(H(:,:,i)' * P(:,:,i + 1) * H(:,:,i))*H(:,:,i)')*r(:,i + 1);
% end
% for i = 2:n + 1
% S2(:,i) = Xhat(i) + P(:,:,i)*r(:,i - 1);
% end
%
% %plot(S2(1,:));
% %vecY = [0 vecY'];
% %plot(vecY);
%%%%%%%%%%%%%%%%OLD CODE%%%%%%%%%%%%%%%%%
%set F to vary correctly with time
% for i = 1:tau1 - 1
% F(:, :, i) = [a 0; 0 1];
% end
% for i = tau1:tau2
% F(:, :, i) = [a 0; 1 0];
% end
% for i = tau2+1:n
% F(:, :, i) = [a 0; 0 1];
% end
%
% for i = 1:n
% vecv(:,:,i) = [v(i) 0];
% end
% vecv = permute(vecv, [2 1 3]);
%
% vecX(:,:,1) = [0 0]';
% for i = 1:n
% vecX(:,:,i + 1) = F(:,:,i) * vecX(:, :, i) + vecv(:, :, i);
% end
%
% for i = 1:n
% H(:, :, i) = [1 0];
% end
% H(:, :, tau) = [0 0];
% H = permute(H, [2 1 3]);
%
% for i = 1:n
% vecY(i) = H(:, :, i)' * vecX(:, :, i + 1);
% end
% vecY = vecY';
%
% Xhat(:,:,1) = [0 0]';
% P(:,:,1) = [1 0; 0 0]; %note: error variance = 1 here
%
% for i = 2:n + 1
% if i ~= tau + 1
% partK = P(:, :, i - 1)*H(:,:,i - 1)*inv( H(:,:,i - 1)' * P(:, :, i - 1) *H(:,:,i - 1));
% Xhatt(:,:,i - 1) = Xhat(:, :, i - 1) + F(:, :, i - 1)* partK * (vecY(i - 1) - H(:, :, i - 1)' * Xhat(:,:, i - 1));
% Xhat(:, :,i) = F(:, :, i - 1)*Xhatt(:, :, i - 1);
% Pt(:,:,i - 1) = P(:,:,i - 1) - partK * H(:,:,i - 1)' *P(:,:,i - 1);
% P(:,:,i) = F(:,:,i - 1)* Pt(:,:,i - 1) * F(:, :, i - 1)' + [1 0; 0 0];
% else
% Xhatt(:,:,i - 1) = Xhat(:, :, i - 1);
% Xhat(:, :,i) = F(:, :, i - 1)*Xhat(:, :, i - 1);
% Pt(:,:, i - 1) = P(:, :, i - 1);
% P(:,:,i) = F(:,:,i - 1)* P(:,:,i - 1) * F(:,:, i - 1)' + [1 0; 0 0];
% end
% end
%
% plot(vecX(1,:),'color','blue')
% hold on
% plot(yplot(tau1 + 2:tau2 + 2),vecX(1,tau1 + 1:tau2 + 1),'color','red');
% %plot (Xhat(1, :))
%
% for i = tau:n
% J(:,:,i) = Pt(:,:,i) *F(:,:,i)' * inv(P(:,:,i + 1));
% end
%
% S(:,:,n + 1) = Xhat(:,:,n + 1);
% for i = 1:n
% S(:,:,n + 1 - i) = Xhatt(:,:,n + 1 - i) + J(:,:,n + 1 - i) * (S(:,:,n + 2 - i) - Xhat(:,:,n + 2 - i));
% end
%
% %plot(S(1,:));
%
%
% r(:,n + 1) = [0 0]';
% for i = n:-1:tau + 1
% r(:,i) = H(:,:,i)*inv(H(:,:,i)' * P(:,:,i + 1) * H(:,:,i)) ...
% + (F(:,:,i) - F(:,:,i)*P(:,:,i + 1)*H(:,:,i)*inv(H(:,:,i)' * P(:,:,i + 1) * H(:,:,i))*H(:,:,i)')*r(:,i + 1);
% end
% r(:,tau) = F(:,:,tau) * r(:,tau + 1);
% for i = tau - 1:-1:1
% r(:,i) = H(:,:,i)*inv(H(:,:,i)' * P(:,:,i + 1) * H(:,:,i)) ...
% + (F(:,:,i) - F(:,:,i)*P(:,:,i + 1)*H(:,:,i)*inv(H(:,:,i)' * P(:,:,i + 1) * H(:,:,i))*H(:,:,i)')*r(:,i + 1);
% end
% for i = 2:n + 1
% S2(:,i) = Xhat(i) + P(:,:,i)*r(:,i - 1);
% end
%
% %plot(S2(1,:));
% %vecY = [0 vecY'];
% %plot(vecY);
⛄ 运行结果
⛄ 参考文献
[1] 王新, 张绍良. 基于MATLAB的卡尔曼滤波在变形监测数据处理中的应用[J]. 2011.
[2] 谭菊华, 王涛. 基于MATLAB实现卡尔曼滤波器的设计[J]. 计算机光盘软件与应用, 2011(7):1.
[3] 毛静. 基于MATLAB的卡尔曼滤波器设计[J]. 软件工程师, 2015(10):2.
