rk4.m

一个关于卫星导航方面的ekf滤波程序

function [tout, yout]=rk4(odefile,tspan,y0)
%   4 Order Runge-Kutta ODE Solver
%   [tout, yout]=rk4(odefile,tspan,y0)
%   odefile is a string variable which indicates the name of the ODE file used.   
%   tspan can be directly written as [starttime endtime step].   
%   y0 is the initial system state.
%   tout and yout returns time vector and state matrix respectively. 
%   Reference: BHU Lib O29-39/01
%   Copyright (c) Shi Heng (2006)

t0=tspan(1);  th=tspan(2);
if length(tspan)<=3,
    h=tspan(3);
else,
    h=tspan(2)-tspan(1);     th=tspan(2);
end
tout=[t0:h:th]';  yout=[];
for t=tout'
    k1=h*eval([odefile,'(t,y0)']);
    k2=h*eval([odefile,'(t+h/2,y0+0.5*k1)']);
    k3=h*eval([odefile,'(t+h/2,y0+0.5*k2)']);
    k4=h*eval([odefile,'(t+h,y0+k3)']);
    y0=y0+(k1+2*k2+2*k3+k4)/6;
    yout=[yout;y0'];
end