%% ARX 模型参数估计：时不变系统与时变系统，比较标准 LS 和带遗忘因子 RLS

clc;

clear;

close all;

%% 参数设置

na = 2; % A(z)阶数

nb = 2; % B(z)阶数

nk = 1; % 输入延迟

N = 1000; % 数据点数

lambda = 0.8; % 遗忘因子

%% 生成时不变系统数据

% 真实参数

a_true = [1.5, 0.7]; % AR 部分系数

b_true = [1.1, 0.3, -0.6]; % X 部分系数

% 生成输入和噪声

u = randn(N, 1);

v = 0.1 * randn(N, 1);

% 初始化输出

y = [0.1 * randn(3, 1); zeros(N-3, 1)];

% 生成输出数据

a_true(1)
for k = 4:N

y(k) = -a_true(1)*y(k-1) - a_true(2)*y(k-2) + ...

b_true(1)*u(k-1) + b_true(2)*u(k-2) + b_true(3)*u(k-3) + v(k);

end

%% 时不变系统参数估计

start_idx = 4

% 递推最小二乘 (RLS)

theta_hat = zeros(5, N);

P_t = 1e4 * eye(5);

innovation = zeros(N,1);

I_eye = eye(5);

for k = start_idx:N

phi = [-y(k-1), -y(k-2), u(k-1), u(k-2), u(k-3)]';

y_k = y(k);

innovation(k)=y_k-phi'*theta_hat(:,k-1);

Lt = P_t*phi/(1+phi'*P_t*phi);

P_t = (I_eye-Lt*phi')*P_t;

theta = theta_hat(:,k-1) + Lt*innovation(k);

theta_hat(:,k) = theta;

end

theta_hat;

% 带遗忘因子 RLS

theta_rls_ff = zeros(5, N);

P_ff = 1e4 * eye(5);

innovation_ff = zeros(N,1);

for k = start_idx:N
 phi = [-y(k-1), -y(k-2), u(k-1), u(k-2), u(k-3)]';

y_k = y(k);

innovation_ff(k)=y_k-phi'*theta_rls_ff(:,k-1);

Lt_ff = P_ff * phi / (lambda + phi'*P_ff*phi);

P_ff = (I_eye-Lt_ff*phi')*P_ff/lambda;

theta_ff = theta_rls_ff(:,k-1) + Lt_ff*innovation_ff(k);

theta_rls_ff(:,k) = theta_ff;

end

%% 时不变系统结果可视化

figure('Name','时不变系统参数估计');

subplot(2,2,1);

plot(theta_hat(1:2,start_idx:end)');

hold on;

plot([1 N], [a_true(1) a_true(1)], 'k--');

plot([1 N], [a_true(2) a_true(2)], 'k--');

title('AR 参数估计');

legend('a1-RLS','a2-RLS','真实值');

subplot(2,2,2);

plot(theta_hat(3:5,start_idx:end)');

hold on;

plot([1 N], [b_true(1) b_true(1)], 'k--');

plot([1 N], [b_true(2) b_true(2)], 'k--');

plot([1 N], [b_true(3) b_true(3)], 'k--');

title('X 参数估计');

legend('b0-RLS','b1-RLS','b2-RLS','真实值');
subplot(2,2,3);

plot(theta_rls_ff(1:2,start_idx:end)');

hold on;

plot([1 N], [a_true(1) a_true(1)], 'k--');

plot([1 N], [a_true(2) a_true(2)], 'k--');

title('AR 参数估计(RLS-FF)');

legend('a1-RLS','a2-RLS','真实值');

subplot(2,2,4);

plot(theta_rls_ff(3:5,start_idx:end)');

hold on;

plot([1 N], [b_true(1) b_true(1)], 'k--');

plot([1 N], [b_true(2) b_true(2)], 'k--');

plot([1 N], [b_true(3) b_true(3)], 'k--');

title('X 参数估计(RLS-FF)');

legend('b0-RLS','b1-RLS','b2-RLS','真实值');

figure('Name','innovation 对比')

subplot(3,1,1);

plot(innovation(start_idx:end)');

subplot(3,1,2);

plot(innovation_ff(start_idx:end)');

subplot(3,1,3);

plot(innovation(start_idx:end)','b');

hold on;

plot(innovation_ff(start_idx:end)','r');
%% 生成时变系统数据

% 时变参数设置

a1_true_tv = [1.5*ones(500,1); 1.3*ones(500,1)]';

a2_true_tv = [0.7*ones(500,1); 0.8*ones(500,1)]';

b1_true_tv = [1.1*ones(500,1); 0.9*ones(500,1)]';

b2_true_tv = [0.3*ones(500,1); 0.6*ones(500,1)]';

b3_true_tv = [-0.6*ones(500,1); -0.7*ones(500,1)]';

% 生成输出数据

y_tv = zeros(N, 1);

for k = 4:N

y_tv(k) = -a1_true_tv(k)*y_tv(k-1) - a2_true_tv(k)*y_tv(k-2) + ...

b1_true_tv(k) *u(k-1) + b2_true_tv(k) *u(k-2) + b3_true_tv(k) *u(k-3) +

end

%% 时变系统参数估计

% 标准 RLS

theta_rls_tv = zeros(5, N);

P_rls_tv = 1e4 * eye(5);

innovation_tv = zeros(N,1);

for k = start_idx:N

phi_tv = [-y_tv(k-1), -y_tv(k-2), u(k-1), u(k-2), u(k-3)]';

ytv = y_tv(k);

innovation_tv(k)=ytv-phi_tv'*theta_rls_tv(:,k-1);

Lt_tv = P_rls_tv*phi_tv/(1+phi_tv'*P_rls_tv*phi_tv);

P_rls_tv = (I_eye-Lt_tv*phi_tv')*P_rls_tv;
 thetaa = theta_rls_tv(:,k-1) + Lt_tv * innovation_tv(k);

theta_rls_tv(:,k) = thetaa;

end

% 带遗忘因子 RLS

% 带遗忘因子 RLS

theta_rls_ff_tv = zeros(5, N);

P_ff_tv = 1e4 * eye(5);

innovation_ff_tv = zeros(N,1);

for k = start_idx:N

phi_ff = [-y_tv(k-1), -y_tv(k-2), u(k-1), u(k-2), u(k-3)]';

y_ftv = y_tv(k);

innovation_ff_tv(k)=y_ftv-phi_ff'*theta_rls_ff_tv(:,k-1);

Lt_ff_tv = P_ff_tv * phi_ff / (lambda + phi_ff'*P_ff_tv*phi_ff);

P_ff_tv = (I_eye- Lt_ff_tv*phi_ff')*P_ff_tv/lambda;

theta_ff_tv = theta_rls_ff_tv(:,k-1) + Lt_ff_tv*innovation_ff_tv(k);

theta_rls_ff_tv(:,k) = theta_ff_tv;

end

%% 时变系统结果可视化

figure('Name','时变系统参数估计');

subplot(2,2,1);

plot(theta_rls_tv(1:2,start_idx:end)');

hold on;

plot(1:N, a1_true_tv, 'k--');

title('AR 参数估计');

legend('a1-RLS','a2-RLS','真实值');

subplot(2,2,2);

plot(theta_rls_tv(3:5,start_idx:end)');

hold on;

plot(1:N, b1_true_tv, 'k--');

plot(1:N, b2_true_tv, 'k--');

plot(1:N, b3_true_tv, 'k--');

title('X 参数估计');

legend('b0-RLS','b1-RLS','b2-RLS','真实值');

subplot(2,2,3);

plot(theta_rls_ff_tv(1:2,start_idx:end)');

hold on;

plot(1:N, a1_true_tv, 'k--');

plot(1:N, a2_true_tv, 'k--');

title('AR 参数估计(RLS-FF)');

legend('a1-RLS','a2-RLS','真实值');

subplot(2,2,4);

plot(theta_rls_ff_tv(3:5,start_idx:end)');

hold on;

plot(1:N, b1_true_tv, 'k--');

plot(1:N, b2_true_tv, 'k--');

plot(1:N, b3_true_tv, 'k--');

legend('b0-RLS','b1-RLS','b2-RLS','真实值');

figure('Name','innovation 对比')

subplot(3,1,1);

plot(innovation_tv(start_idx:end)');

subplot(3,1,2);

plot(innovation_ff_tv(start_idx:end)');

subplot(3,1,3);

plot(innovation_tv(start_idx:end)','b');

hold on;

plot(innovation_ff_tv(start_idx:end)','r');