gcnav.m

来自「GPS TOOLBOX包含以下内容： 1、GPS相关常量和转换因子； 2、角」· M 代码 · 共 59 行

%                          gcnav.m
%  Scope:   This MATLAB macro computes the great circle navigation position,
%           velocity, and acceleration when the ECEF initial position and 
%           velocity are specified; WGS-84 constants are used.
%  Usage:   [pgcm,vgcm,agcm] = gcnav(p0,v0,deltat,nsteps)
%  Description of parameters:
%           p0     - input, ECEF position vector, the components are in meters
%           v0     - input, ECEF velocity vector, the components are in 
%                    meters/second
%           deltat - input, time step, in seconds
%           nsteps - input, number of step desired
%           pgcm   - output, ECEF great circle position, the components are 
%                    in meters; pgcm(k,:) is the k-th time step value
%           vgcm   - output, great circle velocity, the components are 
%                    in meters/second; vgcm(k,:) is the k-th time step value
%           agcm   - output, great circle acceleration, the components 
%                    are in meters/second^2; agcm(k,:) is the k-th time step 
%                    value
%  External Matlab macros used:  uverv, wgs84con
%  Last update:  05/29/00
%  Copyright (C) 1999-00 by LL Consulting. All Rights Reserved.

function  [pgcm,vgcm,agcm] = gcnav(p0,v0,deltat,nsteps)

%  Initialization

deltat2 = 0.5*deltat*deltat;
up = uverv(p0);
pgc = p0;
vgc(1) = (1. - up(1)*up(1))*v0(1) - up(1)*up(2)*v0(2) - up(1)*up(3)*v0(3);
vgc(2) = - up(2)*up(1)*v0(1) + (1. - up(2)*up(2))*v0(2) - up(2)*up(3)*v0(3);
vgc(3) = - up(3)*up(1)*v0(1) - up(3)*up(2)*v0(2) + (1. - up(3)*up(3))*v0(3);

%  Compute angular velocity 

pgcnorm2 = pgc(1)*pgc(1) + pgc(2)*pgc(2) + pgc(3)*pgc(3);
omegagc(1) = (pgc(2)*vgc(3) - pgc(3)*vgc(2)) / pgcnorm2;
omegagc(2) = (- pgc(1)*vgc(3) + pgc(3)*vgc(1)) / pgcnorm2;
omegagc(3) = (pgc(1)*vgc(2) - pgc(2)*vgc(1)) / pgcnorm2;

%  Compute great circle position, velocity, acceleration at each time step

pgcm = zeros(nsteps,3);
vgcm = zeros(nsteps,3);
agcm = zeros(nsteps,3);

for k = 1:nsteps
   
   agc(1) = omegagc(2)*vgc(3) - omegagc(3)*vgc(2);
   agc(2) = - omegagc(1)*vgc(3) - omegagc(3)*vgc(1);
   agc(3) = omegagc(1)*vgc(2) - omegagc(2)*vgc(1);
   pgc = pgc + vgc*deltat + agc*deltat2;
   vgc = vgc + agc*deltat;
   
   pgcm(k,:) = pgc;
   vgcm(k,:) = vgc;
   agcm(k,:) = agc;
  
end