废话不多说,直接上伪代码,matlab版,易于阅读。
基于UD的KF分为两部分:
第一部分实现对残差和v的更新;
第二部分实现对D\U\b三部分的更新。
%% 基于UD分解的KF仿真代码
% ------ Part One Start ------
% 这部分实现 J>1 残差计算和v的更新
for J=n:-1:2
z=z-a(J)*x(J); % residual=delta_z
for K=1:J-1
a(J)=a(J)+U(K,J)*a(K); % f=U^T*a, 赋值后的a(J)就是f
end
v(J)=U(J,J)*a(J);% vi=di*fi
end
% 这部分实现 J=1 残差计算和v的更新
z=z-a(1)*x(1);
v(1)=U(1,1)*a(1);
% ------ Part One Last ------
% ------ Part Two Start------
% 这部分实现 J=1 矩阵D、矩阵U、未加权的KF增益b三部分更新
alpha=r+v(1)*a(1);
gamma=1.0/alpha;
U(1,1)=U(1,1)*r*gamma;
b(1)=v(1);
% 这部分实现 J>1 矩阵D、矩阵U、未加权的KF增益b三部分更新
for J=2:N
beta=alpha; % 将alpha(J-1)代入
alpha=alpha+v(J)*a(J); % 更新alpha(J)
p=-a(J)*gamma; % 将gamma(J-1)代入
gamma=1.0/alpha; % 更新gamma(J)
U(J,J)=U(J,J)*beta*gamma; % 更新D
b(J)=v(J);
for I=1:J-1
beta=U(I,J);
U(I,J)=beta+b(I)*p; % 更新U
b(I)=b(I)+v(J)*beta;% 更新b
end
end
z=z/alpha; % delta_z=delta_z/alpha,即delta_z为加权后的残差
for J=1:N
x(J)=x(J)+b(J)*z; % x_=x+(k)*delta_z=x+(b/alpha)*delta_z,即k=b/alpha,实现了对b的加权
end
% ------ Part Two Last ------