f_turn_inv.m

来自「有关kalman滤波及其一些变形滤波算法」· M 代码 · 共 39 行

%
% Inverse prediction for the coordinated turn model
% used in extended IMM filter demonstration
%
% 

% Copyright (C) 2007 Jouni Hartikainen
%
% This software is distributed under the GNU General Public
% Licence (version 2 or later); please refer to the file
% Licence.txt, included with the software, for details.

function x_k = f_turn_inv(x,param)
    
    dt = param{1};
    w = x(5);
    if w == 0
        coswt = cos(w*dt);
        coswto = cos(w*dt)-1;
        coswtopw = 0;  
        
        sinwt = 0;
        sinwtpw = dt;
    else
        coswt = cos(w*dt);
        coswto = cos(w*dt)-1;
        coswtopw = coswto/w;  
        
        sinwt = sin(w*dt);
        sinwtpw = sinwt/w;
    end
    F = [1 0 sinwtpw    coswtopw  0;...
         0 1 -coswtopw  sinwtpw   0;...
         0 0 coswt      -sinwt    0;...
         0 0 sinwt      coswt     0;...
         0 0 0          0         1];

    x_k = F\x;