KF UD分解之UD分解基础篇【1】

在进行KF UD分解的时候，状态向量的方差-协方差阵P是用UD形式表示的，UD分解是其基础，而此篇文章的目的是能减少大家在进行UD分解的学习时间，如果能起到抛砖引玉的作用就再好不过了。
一、本章学习目的有三：
首先，我们要清楚，什么样的矩阵才是对称正定矩阵？
其次，我们要知道，UD分解的形式如下：P=UDU^T，其中，U是单位上三角矩阵，D是对角矩阵。
最后，我们要能进行代码实现，即给定对称正定矩阵P，可以得到矩阵U和D。
下面为偷懒操作，直接放文档截图了。。。

在matlab中手写UD分解的代码如下，请忽略代码健壮性：

% 实现对称正定矩阵的UD分解
% demo
% U = 1 2 3 4    D = 1 0 0 0    P = 95 66 38 16
%     0 1 2 3        0 3 0 0        66 47 28 12
%     0 0 1 2        0 0 2 0        38 28 18  8
%     0 0 0 1        0 0 0 4        16 12  8  4

P=[95 66 38 16;
    66 47 28 12;
    38 28 18  8;
    16 12  8  4];

% P=[   220   166   113    66    25
%    166   127    88    52    20
%    113    88    63    38    15
%     66    52    38    24    10
%     25    20    15    10     5];

% UD分解原则（从右往左，从下往上，先列后行）
% 由于P是对称正定矩阵，只需要对P的上三角元素做处理
% 1、计算P的矩阵列数
[pi,pj]=size(P);
U=diag(ones(pi,1));
D=diag(ones(pi,1));
if pj>1
    disp('matrix P is ok!')
    for i=pj:-1:1
        Pii=P(i,i);
        P_D_num=pj-i; % 根据Pii来计算Dii时，需要D元素的个数
        u2d=0.0;
        if P_D_num>0            
            ip1=i+1;
            for k=ip1:pj
                u2d=u2d+D(k,k)*U(i,k)*U(i,k);
            end            
        end
        D(i,i)=Pii-u2d; % dii
        disp(['P_col= ',num2str(i)])
        
        
        if i==1
            disp('UD over!') % 第一列
        elseif i==pj
            for j=1:pj-1
                U(j,pj)=P(j,pj)/D(i,i); % 最后一列
            end
        else
            ip1=i+1; % plus 1
            im1=i-1; % minus 1
            
            for k=1:im1;
                disp(['P_row= ',num2str(k)])
                udu=0.0;
                for j=ip1:pj
                    udu=udu+U(k,j)*U(i,j)*D(j,j);
                end
                if j==pj
                    U(k,i)=(P(k,i)-udu)/D(i,i);
                end
            end
        end
        disp('-----------')
    end
end
disp('D')
D
disp('U')
U