Demonstrate Sequential Batch Least-Squares Orbit Determination

Since version 8.
------------------------------------------------------------------------
See also Constant, Mag, RK4, Unit, BatchLSQOD, GroundStation,
SeqBatchLSQOD, El2RV, Period
------------------------------------------------------------------------

Contents

%--------------------------------------------------------------------------
%   Copyright (c) 2008 Princeton Satellite Systems, Inc.
%   All rights reserved.
%--------------------------------------------------------------------------

First generate results from BatchLSQOD

This produces: x0, dx0, P0Inv

nOrbits     = 1;
a           = 7100;
inc         = 95*pi/180;
ecc         = 0.02;
p           = Period(a);
tEnd        = nOrbits*p;
nSim        = 100;
dT          = tEnd/(nSim-1);
x           = zeros(6,nSim);
mu          = Constant('mu earth');
el          = [a inc 0 0 ecc 0];
[r,v]       = El2RV(el,mu);
x(:,1)      = [r;v];
rho         = zeros(1,nSim);
rhoDot      = zeros(1,nSim);

rGSEF       = 6378*Unit([1;0;0.1]); % Ground station pos. in Earth-Fixed frame
omega       = 2*pi/86400;

[rGS, vGS]  = GroundStation(0,omega,rGSEF);
dR     = r - rGS;
dV     = v - vGS;
rho(1)    = Mag(dR);
rhoDot(1) = dR'*dV/rho(1);

% initial guess...
el0         = [1.01*a inc+0.0175 0.01 0.01 ecc-.002 0.01];
[r0,v0]     = El2RV(el0,mu);
x0          = [r0;v0];

Run the simulation to get the measurements

------------------------------------------

r = [r,zeros(3,nSim-1)];
v = [v,zeros(3,nSim-1)];
rGS = [rGS,zeros(3,nSim-1)];
vGS = [vGS,zeros(3,nSim-1)];
t = 0;
for k = 2:nSim
   x(:,k)    = RK4('FOrbCart', x(:,k-1),dT,0,[0;0;0],mu);
   t         = t + dT;
   r(:,k)    = x(1:3,k);
   v(:,k)    = x(4:6,k);
   [rGS(:,k), vGS(:,k)] = GroundStation(t,omega,rGSEF);
   dR        = r(:,k) - rGS(:,k);
   dV        = v(:,k) - vGS(:,k);
   rho(k)    = Mag(dR);
   rhoDot(k) = dR'*dV/rho(k);
end
tvec = (0:(nSim-1))*dT;

ADD RANDOM NOISE to "rho" and "rhoDot" measurements

%----------------------------------------------------
sigmaRho    = 0*5;     % km
sigmaRhoDot = 0*0.5;   % km/s

rho    = rho    + randn(1,nSim)*sigmaRho;
rhoDot = rhoDot + randn(1,nSim)*sigmaRhoDot;

Estimator data structure

%-------------------------
d         = struct;
d.accTime = [];
d.acc     = [];
d.obsTime = tvec;
d.rGS     = rGS;
d.vGS     = vGS;
d.dT      = dT;
d.mu      = mu;
d.name    = 'OrbitODGenObs';

% store initial guess for comparison
x0Ref = x0;

% prepare the measurement vector:
zM = zeros(2*nSim,1);
for i=1:length(zM)/2,
   zM((i-1)*2+1)=rho(i);
   zM((i-1)*2+2)=rhoDot(i);
end

% weighting matrix
W = eye(2*nSim);

Estimate the orbit using Batch Least Squares

%---------------------------------------------
fprintf('\n +++ First calling BatchLSQOD with %d measurements...\n',nSim);
opts = struct('tol',1e-6,'maxIter',10,'verbose',1);   % algorithm options
[x0,dx0,P0,P0Inv] = BatchLSQOD( x0Ref, zM, W, d, opts );

 +++ First calling BatchLSQOD with 100 measurements...
Norm of perturbation, dx0:

	k = 1
	mag(dx0) = 189.018985


	k = 2
	mag(dx0) = 33.275039


	k = 3
	mag(dx0) = 3.505704


	k = 4
	mag(dx0) = 0.358538


	k = 5
	mag(dx0) = 0.000150


	k = 6
	mag(dx0) = 0.000000

Now consider new measurmements

nSimNew = 20;

Run the simulation to get the measurements

------------------------------------------

rGSNew    = zeros(3,nSimNew);
vGSNew    = zeros(3,nSimNew);
rhoNew    = zeros(1,nSimNew);
rhoDotNew = zeros(1,nSimNew);
xNew      = x(:,end);
for k = 1:nSimNew
   xNew    = RK4('FOrbCart', xNew,dT,0,[0;0;0],mu);
   t         = t + dT;
   [rGSNew(:,k), vGSNew(:,k)] = GroundStation(t,omega,rGSEF);
   dRNew        = xNew(1:3) - rGSNew(:,k);
   dVNew        = xNew(4:6) - vGSNew(:,k);
   rhoNew(k)    = Mag(dRNew);
   rhoDotNew(k) = dRNew'*dVNew/rhoNew(k);
end
tvecNew = tvec(end) + dT + (0:(nSimNew-1))*dT;

ADD RANDOM NOISE to "rhoNew" and "rhoDotNew" measurements

%----------------------------------------------------------
rhoNew    = rhoNew    + randn(1,nSimNew)*sigmaRho;
rhoDotNew = rhoDotNew + randn(1,nSimNew)*sigmaRhoDot;

% new measurement vector
zMNew = zeros(2*nSimNew,1);
for i=1:length(zMNew)/2,
   zMNew((i-1)*2+1)=rhoNew(i);
   zMNew((i-1)*2+2)=rhoDotNew(i);
end

% new weighting matrix
WNew = eye(2*nSimNew);

New estimator data structure

%-------------------------
dNed         = struct;
dNew.accTime = [];
dNew.acc     = [];
dNew.obsTime = tvecNew;
dNew.rGS     = rGSNew;
dNew.vGS     = vGSNew;
dNew.dT      = dT;
dNew.mu      = mu;
dNew.name    = 'OrbitODGenObs';
opts.tol = 1e-8;

Call sequential batch least squares algorithm

fprintf('\n +++ Now calling SeqBatchLSQOD with %d new measurements...\n',nSimNew);
[x0New,dx0New,P0New,P0InvNew] = SeqBatchLSQOD( dx0, P0Inv, x0, zMNew, WNew, dNew, opts );


%--------------------------------------

 +++ Now calling SeqBatchLSQOD with 20 new measurements...
Norm of perturbation, dx0:
     9.092251079496220e-08
     1.097394853277278e-07
     1.148404596068075e-07
     9.060391668153939e-08
     7.608991265606016e-08
     1.084609296055088e-07
     9.958572395034863e-08
     1.081496849768412e-07
     1.208089435692382e-07
     9.612085906246952e-08

Published with MATLAB® R2019a