table6pt8test.m

Kalman Filtering- Theory and Practice Using MATLAB 2008年第三版的随书源码

%
% Grewal & Andrews, Kalman Filtering Theory and Practice Using MATLAB, 3rd
% Edition, Wiley, 2008.
%
% Test of algorithms listed in Table 6.8, for inversion of symmetric
% positive definite matrices in place, overwriting the input with the
% output.
%
% Test matrices of dimension 1 to 12 are generated using random number
% generator randn, with the number of samples equal to the dimension
% of the matrix to be generated.  (This can result in deficient
% pseudorank.)
%
for m=1:12,
    M = zeros(m);
    X = zeros(m);
    for k=1:m,
        v = randn(m,1);
        M = M + v*v';
    end;
    UD = M;
    UD = UD_decomp(UD);
    for i=1:m,
        for j=i:m,
            if i==j
                s = UD(i,i);
            else
                s = UD(i,j)*UD(j,j);
            end;
            for k=j+1:m,
                s = s + UD(i,k)*UD(k,k)*UD(j,k);
            end;
            X(i,j) = s;
            X(j,i) = X(i,j);
        end;
    end;
    e1 = max(max(abs((X-M)/M)));
    disp(['Dimension = ',num2str(m)]);
    disp(['    Max error in UD decomp in-place = ',num2str(e1)]);
    D = zeros(m);
    U = UD;
    for i=1:m;
        D(i,i) = UD(i,i);
        U(i,i) = 1;
    end;
    UD = UDinv(UD);
    %
    Di = zeros(m);
    Ui = UD;
    for i=1:m;
        Di(i,i) = UD(i,i);
        Ui(i,i) = 1;
    end;
    e2 = max(max(abs(D*Di-eye(m))));
    disp(['    Max err in D inversion in-place = ',num2str(e2)]);
    e3 = max(max(abs(U*Ui-eye(m))));
    disp(['    Max err in U inversion in-place = ',num2str(e3)]);
    M0 = M;
    M = SPDinvIP(M);
    e4 = max(max(abs(M0*M-eye(m))));
    disp(['    Max err in M inversion in-place = ',num2str(e4)]);
end;