维纳滤波器 卡尔曼系列滤波器以及自适应LMS RLS滤波器matlab代码实现

warning('off');

clear;clc;

x = normrnd(5,2,1,2000);

index = 5;

if index == 1 % 自适应滤波LMS

N = 50;

W = zeros(N,1);

d = 5;

miu = (0+1/(29*N))*0.5;

x_hat_set = zeros(1,2000);

for i = 1:2000

x_n_hat = x(i);

if i >= N

X = x(i:-1:i-N+1)';

x_n_pred = X'*W;

W = W+2*miu*(d-x_n_pred)*X;

x_n_hat = X'*W;

end

x_hat_set(i) = x_n_hat;

end

figure(1)

plot(1:2000,x,1:2000,x_hat_set);

delta = abs(x_hat_set-5);

figure(2)

plot(1:2000,delta);

var(x_hat_set(100:end))

elseif index == 2 % 卡尔曼滤波/扩展卡尔曼滤波

Q = 1e-5;

R = 4;

x_nb_hat = 5;

F = 1;

H = 1;

M = 1;

x_n_hat_set = zeros(1,2000);

for j = 1:2000

x_n_pred = F*x_nb_hat;

M_pred = F'*M*F+Q;

K = M_pred*H'/(H*M_pred*H'+R);

x_n_hat = x_n_pred+K*(x(j)-H*x_n_pred);

M = (1-K*H)*M_pred;

x_nb_hat = x_n_hat;

x_n_hat_set(j) = x_n_hat;

end

figure(1)

plot(1:2000,x,1:2000,x_n_hat_set);

delta = abs(x_n_hat_set-5);

figure(2)

plot(1:2000,delta);

var(x_n_hat_set(100:end))

elseif index == 3 % 维纳滤波

N = 50;

y_set = zeros(1,2000);

for i = 1:2000

y = x(i);

if i >= N

rxx = x(i-N+1:i)'*x(i-N+1:i);

rxs = 5*x(i-N+1:i)';

h = rxx\rxs;

y = x(i-N+1:i)*h;

end

y_set(i) = y;

end

figure(1)

plot(1:2000,x,1:2000,y_set);

delta = abs(y_set-5);

figure(2)

plot(1:2000,delta);

var(y_set(100:end))

elseif index == 4 % 无迹卡尔曼滤波

Q = 1e-5;

R = 4;

x_nb_hat = 5;

F = 1;

H = 1;

M = 1;

x_n_hat_set = zeros(1,2000);

lambda = 1;

w1 = lambda/(1+lambda);

w2 = 1/(2*(1+lambda));

w3 = 1/(2*(1+lambda));

for j = 1:2000

x1 = x_nb_hat;

x2 = x_nb_hat+sqrt((1+lambda)*M);

x3 = x_nb_hat-sqrt((1+lambda)*M);

x_1 = x1; % f=1

x_2 = x2;

x_3 = x3;

x_n_pred = w1*x_1+w2*x_2+w3*x_3;

M_pred = w1*(x_1-x_n_pred)^2+w2*(x_2-x_n_pred)^2+w3*(x_3-x_n_pred)^2+Q;

x1_ = x_n_pred;

x2_ = x_n_pred+sqrt((1+lambda)*M_pred);

x3_ = x_n_pred-sqrt((1+lambda)*M_pred);

x_1_ = x1_; % g=1

x_2_ = x2_;

x_3_ = x3_;

miu_obs = w1*x_1_+w2*x_2_+w3*x_3_;

sigma_obs = w1*(x_1_-miu_obs)^2+w2*(x_2_-miu_obs)^2+w3*(x_3_-miu_obs)^2+R;

rxs = w1*(x_1-x_n_pred)*(x_1_-miu_obs)+w2*(x_2-x_n_pred)*(x_2_-miu_obs)+w3*(x_3-x_n_pred)*(x_3_-miu_obs);

K = rxs/sigma_obs;

x_n_hat = x_n_pred+K*(x(j)-miu_obs);

M = M_pred-K*sigma_obs'*K';

x_nb_hat = x_n_hat;

x_n_hat_set(j) = x_n_hat;

end

figure(1)

plot(1:2000,x,1:2000,x_n_hat_set);

delta = abs(x_n_hat_set-5);

figure(2)

plot(1:2000,delta);

var(x_n_hat_set(100:end))

elseif index == 5 % 粒子滤波

Q = 1e-5;

R = 4;

x_hat_set = zeros(1,2000);

V = 1;

N = 100; % 粒子数目

p_nb = normrnd(5,sqrt(V),1,N);

p_n = zeros(1,N);

p_n_tilde = zeros(1,N);

w = zeros(1,N);

for i = 1:2000

for j = 1:N

p_n(j) = normrnd(p_nb(j),sqrt(Q));

w(j) = exp(-(x(i)-p_n(j))^2/(2*R));

end

w = w/sum(w);

for m = 1:N % 重采样，之后粒子权重都为1/N

p_n_tilde(m) = p_n(find(rand <= cumsum(w),1));

end

w(1:end) = 1/N;

x_hat_set(i) = mean(p_n_tilde); % 状态估计

p_nb = p_n_tilde;

end

figure(1)

plot(1:2000,x,1:2000,x_hat_set);

delta = abs(x_hat_set-5);

figure(2)

plot(1:2000,delta);

var(x_hat_set(100:end))

elseif index == 6 % 自适应滤波RLS

N = 50;

P_nb = eye(N);

W = zeros(N,1);

x_hat_set = zeros(1,2000);

lambda = 0.95;

for i = 1:2000

x_n_hat = x(i);

if i >= N

X = x(i:-1:i-N+1)';

x_n_pred = X'*W;

e = 5-x_n_pred;

P_n = 1/lambda*P_nb-(1/lambda)^2*P_nb*X*X'*P_nb/(1/lambda*X'*P_nb*X+1);

W = W+P_n*X*e;

x_n_hat = X'*W;

end

x_hat_set(i) = x_n_hat;

end

figure(1)

plot(1:2000,x,1:2000,x_hat_set);

delta = abs(x_hat_set-5);

figure(2)

plot(1:2000,delta);

var(x_hat_set(100:end))

end

如有问题请多指教。希望通过相互讨论加深对滤波器的理解O(∩_∩)O