Home > src > demos > eimm_demo > f_turn.m

f_turn

PURPOSE ^

A coordinated turn model for extended IMM filter demonstration

SYNOPSIS ^

function x_k = f_turn(x,param)

DESCRIPTION ^

 A coordinated turn model for extended IMM filter demonstration

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

0001 %
0002 % A coordinated turn model for extended IMM filter demonstration
0003 %
0004 %
0005 
0006 % Copyright (C) 2007 Jouni Hartikainen
0007 %
0008 % This software is distributed under the GNU General Public
0009 % Licence (version 2 or later); please refer to the file
0010 % Licence.txt, included with the software, for details.
0011 
0012 function x_k = f_turn(x,param)
0013     
0014     dt = param{1};
0015     w = x(5);
0016     if w == 0
0017         coswt = cos(w*dt);
0018         coswto = cos(w*dt)-1;
0019         coswtopw = 0;  
0020         
0021         sinwt = sin(w*dt);
0022         sinwtpw = dt;
0023     else
0024         coswt = cos(w*dt);
0025         coswto = cos(w*dt)-1;
0026         coswtopw = coswto/w;  
0027         
0028         sinwt = sin(w*dt);
0029         sinwtpw = sinwt/w;
0030     end
0031         
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     x_k = F*x;
0038     
0039     
0040     
0041     
0042

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