Home > src > demos > eimm_demo > f_turn_inv.m

f_turn_inv

PURPOSE ^

Inverse prediction for the coordinated turn model

SYNOPSIS ^

function x_k = f_turn_inv(x,param)

DESCRIPTION ^

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

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

0001 %
0002 % Inverse prediction for the coordinated turn model
0003 % used in extended IMM filter demonstration
0004 %
0005 %
0006 
0007 % Copyright (C) 2007 Jouni Hartikainen
0008 %
0009 % This software is distributed under the GNU General Public
0010 % Licence (version 2 or later); please refer to the file
0011 % Licence.txt, included with the software, for details.
0012 
0013 function x_k = f_turn_inv(x,param)
0014     
0015     dt = param{1};
0016     w = x(5);
0017     if w == 0
0018         coswt = cos(w*dt);
0019         coswto = cos(w*dt)-1;
0020         coswtopw = 0;  
0021         
0022         sinwt = 0;
0023         sinwtpw = dt;
0024     else
0025         coswt = cos(w*dt);
0026         coswto = cos(w*dt)-1;
0027         coswtopw = coswto/w;  
0028         
0029         sinwt = sin(w*dt);
0030         sinwtpw = sinwt/w;
0031     end
0032     F = [1 0 sinwtpw    coswtopw  0;...
0033          0 1 -coswtopw  sinwtpw   0;...
0034          0 0 coswt      -sinwt    0;...
0035          0 0 sinwt      coswt     0;...
0036          0 0 0          0         1];
0037 
0038     x_k = F\x;

Generated on Fri 12-Aug-2011 15:15:16 by m2html © 2005