KF UD分解之伪代码实现进阶篇【2】

废话不多说，直接上伪代码，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 ------