triag.m

是卡尔曼滤波算法的源代码

function S = triag(A)
% TRIAG
% -----
%     S=triag(A) uses a Householder transformation of the rectangular
%     matrix A to produce a square and upper triangular matrix S with
%     the property S*S' = A*A'.

%     Literature:
%     The algorithm is described in Grewal & Andrews: "Kalman Filtering - 
%     Theory and Practice" Prentice-Hall, 1993, pp. 234. However, there 
%     are certain cases in which their implementation fails. These have been
%     handled as described in G.W. Stewart: "Matrix Algorithms, 
%     Vol. 1: Basic Decompositions", SIAM 1988.
%
% Written by: Magnus Norgaard, IMM/IAU, Technical University of Denmark
% LastEditDate: Apr. 15, 2000

[n,rn]=size(A);          % Rows and columns of A
r = rn-n;
v = zeros(n,1);
u = zeros(1,rn);
S = zeros(n,n);

for k=n:-1:1,
   u(1:r+k) = A(k,1:r+k);
   ny = norm(u(1:r+k));
   if ny==0,
     u(r+k) = sqrt(2);
   else
      u(1:r+k) = u(1:r+k)/ny;
      if u(r+k)>=0,  
        u(r+k) = u(r+k) + 1;
        ny = -ny;
      else
        u(r+k) = u(r+k) - 1;
      end
      u(1:r+k) = u(1:r+k)/sqrt(abs(u(r+k)));
   end
   v(1:k-1,1) = A(1:k-1,1:r+k)*u(1,1:r+k)';
   A(1:k-1,1:r+k) = A(1:k-1,1:r+k) - v(1:k-1,1)*u(1,1:r+k);
   S(1:k-1,k) = A(1:k-1,r+k);
   S(k,k)=ny;
end