rungekutta.asv

高速车辆组合导航研究

 function PX =RungeKutta(tf,X0,T)  
% anatomise the differential equation and get the discrete state equation
% which is noliner
% 11-24-2007
% Output:
%        X:       solution of the differential equation   (N,M)
% Input:
%     fn_Y:       the differential equation
%        Y:       [x,y,z,vx,vy,vz]'
%       X0:       the initial value of Y 
%        N:       the dimension of the differential equation
%        M:
%        h:       the internal time(the step length) 
%       tf:       the longth of the time

% Reference:
% Chapter 7 常微分方程初值问题的数值解法
% 《数值分析》北京航空航天大学出版社
% clc;
% clear;
% tf = 45000;%final time
% T = 3;%time interval
% x0 = [4.590e6,4.388e6,3.228e6,-4.612e3,5.014e2,5.876e3]';% initial true state;
% Y0=x0;
% K=[];


i = 1;
X = X0;
XX = X0;
PX = [];
t0=0;

h =1;
N = 6;

for tt = 0:T:tf
    
    PX(:,i) = X; %save the result
    
    t=t0+tt;
    k1=funy(X);
    
    t=t0+tt+1/2*h;
    X(1)=XX(1)+1/2*h*k1(1);
    X(2)=XX(2)+1/2*h*k1(2);
    X(3)=XX(3)+1/2*h*k1(3);
    X(4)=XX(4)+1/2*h*k1(4);
    X(5)=XX(5)+1/2*h*k1(5);
    X(6)=XX(6)+1/2*h*k1(6);
%     k2=[fun_Y(1);fun_Y(2);fun_Y(3);fun_Y(4);fun_Y(5);fun_Y(6)];
    k2=funy(X);
    
    
    t=t0+tt+1/2*h;
    X(1)=XX(1)+1/2*h*k2(1);
    X(2)=XX(2)+1/2*h*k2(2);
    X(3)=XX(3)+1/2*h*k2(3);
    X(4)=XX(4)+1/2*h*k2(4);
    X(5)=XX(5)+1/2*h*k2(5);
    X(6)=XX(6)+1/2*h*k2(6);
%     k3=[fun_Y(1);fun_Y(2);fun_Y(3);fun_Y(4);fun_Y(5);fun_Y(6)];
    k3=funy(X);
    
    t=t0+tt+h;
    X(1)=XX(1)+h*k3(1);
    X(2)=XX(2)+h*k3(2);
    X(3)=XX(3)+h*k3(3);
    X(4)=XX(4)+h*k3(4);
    X(5)=XX(5)+h*k3(5);
    X(6)=XX(6)+h*k3(6);
%     k4=[fun_Y(1);fun_Y(2);fun_Y(3);fun_Y(4);fun_Y(5);fun_Y(6)];
    k4=funy(X);
    
    X=X+h/6*(k1+2*k2+2*k3+k4);
    XX = X;
   
    i=i+1;
   
end