model_3d_right.m

this is 3d tr model for airplane target tracking

function [X_k] = Model_3D_Right(X_k_1 , T , Q)

sizex = size(X_k_1);

X_k = zeros ( sizex );

for i = 1 : sizex (1 , 2)

    omega_k =  norm( X_k_1( 6:9 , i) ) / norm( X_k_1( 4:6 , i) ) ;

    F_k = [eye(3 , 3) , sin(omega_k * T ) * eye(3 , 3) , omega_k ^ -2 * (1 - cos(omega_k * T)) * eye(3 , 3);  
           zeros(3 , 3) , cos( omega_k * T ) * eye(3 , 3) , sin( omega_k * T ) / omega_k * eye(3 , 3) ;
           zeros(3 , 3) , -omega_k * sin( omega_k * T ) * eye(3 , 3) , cos(omega_k * T) * eye(3, 3)];
        
        

    X_k(: , i) = F_k * X_k_1(: , i) ;
end

%v_cauchy = cauchy(q,State_Number,N);

v_gaussian = Q^.5 * randn (3 , sizex(1,2));

G_k = [ .5 * T ^ 2 * eye(3 , 3) ; T * eye(3 ,3) ; eye(3 , 3) ] ;

X_k = X_k + G_k * v_gaussian ; 

return