LMS、kalman、RLS的Matlab仿真
LMS、kalman、RLS的Matlab仿真
- author:LMS和RLS部分:hwb
- 图和代码如下,细节以后再补:
画图部分
clear all;
clc;
%% 参数设置
sub_fre = 5;
fs=4096;
Ord = 48; % 权重阶数
N = 500; % 迭代次数
K = 100; % 数据截取
%% 仿真数据产生
m=0:(fs-1);
f_sub = linspace(0.4,0.5,sub_fre);
A = ones(sub_fre,1);
xn = zeros(fs,1)';
for i=1:sub_fre
xn = xn + 1*A(i)*sin(2*pi*f_sub(i)*m); %产生含有噪声的序列xn
end
dn = 1*sin(2*pi*0.9*m);
xn = xn + dn + 1*randn(1,fs);
%% LMS实现过程
% function [xn_p,err_n] = rls_self(xn, dn,lambda,delta,Ord,N,K)
[xn_p_lms, err_n_lms] = lms_self(xn, dn, Ord, 0.001, N, K);
M = 1:K;
dn_m = dn(Ord+1:Ord+K);
xn_m = xn(Ord+1:K+Ord);
subplot(3,2,1);
plot(M, xn_p_lms,M,dn_m,M,xn_m,'linewidth',1.25);
legend("滤波后信号", "理想信号", "原始信号");
% xlabel("N(点数)");
ylabel("幅值");
title("信号波形对比");
M = 1:N;
subplot(3,2,2);
plot(M, err_n_lms);
title("均方误差变化");
%% 归一化LMS算法
N2 = 3000;
lms_a = 10;
lms_b = 1.5;
lms_mu = lms_a/(lms_b + xn*xn');
[~, err_n_lms] = lms_self(xn, dn, Ord, lms_mu, N2, K);
subplot(3,2,3);
M = 1:N2;
plot(M, err_n_lms,'*-');
title("归一化LMS算法 均方误差");
%% 不同步长的影响 10^-2 - 10^-5 取100个点
mu_N = 100;
mu_ch = linspace(10^-2,10^-4,mu_N);
mu_err = zeros(mu_N,1);
for i=1:mu_N
[~, err_n_lms] = lms_self(xn, dn, Ord, mu_ch(i), 500, K);
mu_err(i) = err_n_lms(end);
disp(mu_err(i));
end
subplot(3,2,4);
plot(mu_ch, mu_err,'*-');
title("不同步长的影响(N=1000)");
suptitle('1-4 LMS算法 5-6 RLS算法');
%% RLS
lambda = 0.998;
delta = 10^-4;
[xn_p,err_n] = rls_self(xn, dn,lambda,delta,Ord,N,K);
dn_m = dn(Ord+1:Ord+K);
xn_m = xn(Ord+1:K+Ord);
subplot(3,2,5);
M = 1:K;
plot(M, xn_p,M,dn_m,M,xn_m,'linewidth',1.25);
legend("滤波后信号", "理想信号", "原始信号");
title("RLS信号波形对比");
M = 1:N;
subplot(3,2,6);
plot(M, err_n);
title("RLS均方误差变化");
%function [xn_p,err_n] = rls_self(xn, dn,lambda,delta,Ord,N,K)
LMS实现-Matlab仿真
function [xn_p, err_n] = lms_self(xn,dn,Ord,mu,N,K)
%LMS_SELF 此处显示有关此函数的摘要
% 此处显示详细说明
wn = zeros(Ord,1);
en = 0;
err_n = zeros(N,1);
%% 迭代
for i=1:N % 迭代次数
xn_f = xn(i:Ord+i-1)'; % filter中各抽头的系数
en = dn(Ord+i) - wn'*xn_f;
wn = wn + mu*en.*xn_f;
for j=1:N % 迭代次数
xn_f = xn(j:Ord+j-1)'; % filter中各抽头的系数
en = dn(Ord+j) - wn'*xn_f;
err_n(i) = err_n(i) + abs(en)^2;
end
end
xn_p = zeros(K,1);
for j=1:K % 迭代次数
xn_f = xn(j:Ord+j-1)'; % filter中各抽头的系数
xn_p(j) = wn'*xn_f;
end
end
RLS实现-Matlab仿真
function [xn_p,err_n] = rls_self(xn, dn,lambda,delta,Ord,N,K)
%% 初始化权重系数->RLS
wn = zeros(Ord,1);
Pn = (1/delta).*eye(Ord);
kn = zeros(Ord,1);
err_n = zeros(N,1);
%% 迭代
for i=1:N % 迭代次数
xn_f = xn(i:Ord+i-1)'; % filter中各抽头的系数
en = dn(Ord+i) - wn'*xn_f;
kn = (Pn*xn_f)/(lambda + xn_f'*Pn*xn_f);
Pn = (1/lambda)*(Pn-kn*xn_f'*Pn);
wn = wn + kn*en;
for j=1:N % 迭代次数
xn_f = xn(j:Ord+j-1)'; % filter中各抽头的系数
en = dn(Ord+j) - wn'*xn_f;
err_n(i) = err_n(i) + abs(en)^2;
end
end
xn_p = zeros(K,1);
for j=1:K % 迭代次数
xn_f = xn(j:Ord+j-1)'; % filter中各抽头的系数
xn_p(j) = wn'*xn_f;
end
end
卡尔曼滤波的Matlab实现
- 声明:卡尔曼滤波与本次信号模型不太搭,故此代码参考自朋友
clear all; close all; clc;
% 等速模型卡尔曼滤波仿真Kalman Filter
%% Initial condition
ts = 1; % 采样时间间隔
t = [0:ts:100]; % 采样时刻
T = length(t); % 采样点数
%% Initial state
x = [0 40 0 20]';
%% Process noise covariance 过程噪声协方差矩阵
q = 5; % 过程噪声协方差
Q1 = q*eye(4); % 过程噪声协方差矩阵
%% Measurement noise covariance 观测噪声协方差矩阵
r = 5; % 观测噪声协方差
Q2 = r*eye(2); % 观测噪声协方差矩阵
%% Process and measurement noise
v1 = sqrt(Q1)*randn(4,T); % 过程噪声序列 维度：4 x T
v2 = sqrt(Q2)*randn(2,T); % 观测噪声序列 维度：2 x T
%% Estimate error covariance initialization
p = 5; % 估计误差协方差矩阵
P(:,:,1) = p*eye(4); % 估计误差的协方差矩阵
%==========================================================================
%% 连续时间状态空间模型
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];
%% 离散时间状态空间模型
% F:状态转移矩阵
% H:观测矩阵
sysc = ss(A,B,C,D); % 生成连续时间状态空间模型
sysd = c2d(sysc, ts, 'zoh'); % 有连续时间状态空间模型 到 离散时间状态空间模型
[F G C I] = ssdata(sysd); % 取出离散时间状态空间模型中的；过程方程的 F:状态转移矩阵；观测方程中的H:观测矩阵
%% Practice state of target
for i = 1:T-1
x(:,i+1) = F*x(:,i);
end
x = x + v1; % 噪声下的估计向量序列
y = C*x + v2; % 噪声下的观测向量序列
%==========================================================================
%% Kalman Filter
% 书P179 算法5.4.1 kalman 自适应滤波算法
K = P;
x_hat = [0 0 0 0].';
% x_hat = x;
for i = 1:T-1
G = F * K * C'* inv(C * K * C' + Q2); % 书 P179 式5.4.24所定义 算法5.4.1中给出计算方法
alpha = y(:, i) - C * x_hat(:, i);
x_hat(:, i + 1) = F * x_hat(:, i) + G * alpha; % 计算当前状态向量 x_hat
P = K - inv(F) * G * C * F;
K = F * P * F' + Q1; % 更新 kalman 增益
end
%% LMS adaptive filter
% 书P183 算法5.5.1 LMS自适应滤波及其基本变形
omega(:, :, 1) = zeros(4, 4);
x_hat2(:, 1) = x(:, 1);
for i = 1:T - 1
x_hat2(:, i + 1) = omega(:, :, i)' * x_hat2(:, i); % 得到当前估计值
e(:, i) = x(:, i + 1) - x_hat2(:, i + 1); % 计算估计值和实际值的误差
alpha = 1 / (0.5 + x(:, i)' * x(:, i));
omega(:, :, i + 1) = omega(:, :, i) + 10 * x_hat2(:, i) * conj(e(i))'; % 更新权向量
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;
plot(t, x_hat2(1, :), "k"), grid on
title('Practical and Tracking Position on X axis')
legend('Practical Position', 'Kalman Tracking Position', 'LMS 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;
plot(t,x_hat2(2,:),'k'),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;
结语
稍微有点赶,