linearize.m

来自「一个关于卫星导航方面的ekf滤波程序」· M 代码 · 共 60 行

function out = linearize(state, V, N, matri)
% EKF-Linearize Function
% Input: Current state, Observation noise, Time of calcu, 
%        Matrix needs to be generated
% Output: Linearized Matrix
% (c) Shi Heng (2007)

  epsilon = 1e-8;                    % 小步长
  
  switch (matri)
    case 'A'      % A = dF/dX
      A  = zeros(6,6);      
      f1 = feval(@ffun,state,V);
      for j=1:6,
        s = state;       
        s(j) = s(j) + epsilon;
        f2 = feval(@ffun,s,V);
        A(:,j) = (f2-f1)/epsilon;
      end
      out = A;


    case 'C'      % C = dH/dX
      C = zeros(6,6);      
      f3 = feval(@hfun,state,N);
      for j=1:6,
        s = state;
        s(j) = s(j) + epsilon;
        f4 = feval(@hfun,s,N);
        C(:,j) = (f4-f3)/epsilon;
      end
      out = C;


    case 'G'      % G = dF/dv
      G = zeros(6,6);      
      f1 = feval(@ffun,state,V);
      for j=1:6,
        Vtemp = V;
        Vtemp(j) = Vtemp(j) + epsilon;
        f5 = feval(@ffun,state,Vtemp);
        G(:,j) = (f5-f1)/epsilon;
      end
      out = G;


    case 'H'      % H = dH/dn
      H = zeros(6,6);      
      f3 = feval(@hfun,state,N);
      for j=1:6,
        Ntemp = N;
        Ntemp(j) = Ntemp(j) + epsilon;
        f6 = feval(@hfun,state,Ntemp);
        H(:,j) = (f6-f3)/epsilon;
      end
      out = H;

  otherwise
      error('Linearization failed!');
  end