triag.m

国外一款非线性估计的工具箱

function M = triag(N)
% Householder triangularization,
% N is rectangular matrix, M is square matrix,
% M is determined so that the equality  M*M' = N*N' holds

% References:
% G.W. Steward (1998): Matrix Algorithms, Vol. I: Basic Decomposition.
% M.S. Grewal, A.P. Andrews (2001): Kalamn Filtering: Theory and Practise Using MATLAB.
% M. Noorgard, N.K. Poulsen, O. Ravn (2000): KALMTOOL.
% G.H. Golub, C.F. van Loan (1996): Matrix Computation, 3rd Edition.

% Nonlinear Filtering Toolbox version 2.0-rc4
% Copyright (c) 1995 - 2007 NFT developement Team,
%              Department of Cybernetics,
%              University of West Bohemia in Pilsen

%initialization
[a,b]=size(N);
dif = b-a;
v = zeros(a,1);
u = zeros(1,b);
M = zeros(a,a);

%Householder triangularization
for i = a:-1:1,
    m(1:dif+i) = N(i,1:dif+i);
    ny = norm(m(1:dif+i));
    if ny == 0,
        m(dif+i) = sqrt(2);
    else
        m(1:dif+i) = m(1:dif+i)/ny;
        if m(dif+i) >= 0,
            m(dif+i) = m(dif+i) + 1;
            ny = -ny;
        else
            m(dif+i) = m(dif+i) - 1;
        end
        m(1:dif+i) = m(1:dif+i)/sqrt(abs(m(dif+i)));
    end
    v(1:i-1,1) = N(1:i-1,1:dif+i)*m(1,1:dif+i)';
    N(1:i-1,1:dif+i) = N(1:i-1,1:dif+i) - N(1:i-1,1:dif+i)*m(1,1:dif+i)'*m(1,1:dif+i);
    M(1:i-1,i) = N(1:i-1,dif+i);
    M(i,i) = ny;
end