维纳滤波器、卡尔曼系列滤波器以及自适应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

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

【角点检测】基于各向异性高斯方向导数滤波器实现图像角点检测附matlab代码
1 内容介绍为了改进噪声鲁棒性和定位准确性,利用各向异性高斯方向导数滤波器,提出多方向角点检测算法.该算法利用一组各向异性高斯方向导数滤波器对输入图像进行卷积处理得到各个方向的滤波器响应.对于每个像 ...
【滤波跟踪】基于变分贝叶斯自适应卡尔曼滤波器VPAKF实现无人机状态估计附matlab代码
✅作者简介:热爱科研的Matlab仿真开发者,修心和技术同步精进,matlab项目合作可私信.
【滤波跟踪】基于粒子、sigma和卡尔曼滤波器实现目标跟踪滤波附matlab代码
✅作者简介:热爱科研的Matlab仿真开发者,修心和技术同步精进,matlab项目合作可私信.
滤波总结+应用举例1(kalman滤波、序贯滤波、UD分解滤波、遗忘滤波、信息滤波、自适应滤波)（内含matlab代码）
最近在研究滤波的相关知识,在此写个总结,以免自己遗忘. 卡尔曼滤波其实是一种估计方法,在已知前一时刻的状态量和当前时刻的量测情况下,估计当前状态量. 1.首先介绍常规kalman filter(含详细 ...
Matlab自适应均线_李洋：卡夫曼自适应移动平均线MATLAB代码
SMA:Simple MA 简单平均线 EMA:Exp MA指数平均线 AMA:Adaptive MA 卡夫曼自适应移动平均算法过程卡夫曼自适应移动平均算法过程整理 http://www.matl ...
matlab滤波器fdatool,各种类型滤波器设计(fdatool，原理，matlab代码)
数据处理对于一组数据,只有时间戳和加速度,怎么样进行傅立叶变换分析? 参考信号处理内容,首先模拟一组数据进行分析. 以下数据两个频率为1Hz与100Hz,经过采样和傅立叶变化之后,捕捉到信号对应的频 ...
LMS学习函数MATLAB代码
clear,clc close all P=-5:5; d=3*P-7; randn('state',2); d=d+randn(1,length(d))*1.5 P=[ones(1,length(P ...
自适应蝴蝶算法附matlab代码IBOA
✅作者简介:热爱科研的Matlab仿真开发者,修心和技术同步精进,matlab项目合作可私信.
基于NLMS 算法的自适应信道均衡附matlab代码
✅作者简介:热爱科研的Matlab仿真开发者,修心和技术同步精进,matlab项目合作可私信.
基于MPC的自适应巡航控制（Matlab代码实现）

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

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

最新文章

热门文章