cnscl.m

前人用四阶龙格库塔方法进行微分方程解算,用matlab编写的源代码,主要用于四元素微分方程的实时解算

function [phim, dvbm] = cnscl(wm, vm)
global glv
    n = size(wm,2);
    cm = [0;0;0]; sm = [0;0;0];
    for k=1:n-1
        cm = cm + glv.cs(n-1,k)*wm(:,k); %准备圆锥误差补偿
    end
    wmm = sum(wm,2);
    phim = wmm + cross(cm,wm(:,n));
    if nargin==2
        for k=1:n-1
            sm = sm + glv.cs(n-1,k)*vm(:,k); %准备划船误差补偿
        end
        vmm = sum(vm,2);
	    dvbm = vmm + 1.0/2*cross(wmm,vmm) + (cross(cm,vm(:,n))+cross(sm,wm(:,n)));
    end