Relative orbit control using LQG

Note: This demo requies the Mu-Analysis and Synthesis Toolbox.

------------------------------------------------------------------------
See also AC, QCE, QCR, LinOrbLQG, DiscreteHills, LPCircular, LinOrb,
OrbRate, Period
------------------------------------------------------------------------

Combine the estimator and regulator into one controller

------------------------------------------------------------------------- Copyright (c) 2004 Princeton Satellite Systems, Inc. All rights reserved. Since version 7. ------------------------------------------------------------------------- 24-Feb-2017: Update output from LinOrbLQG (4 matrices) -------------------------------------------------------------------------

if( ~exist('pck','file') | ~exist('sysic','file') )
   msgbox('The mu-Analysis and Synthesis toolbox is required for this demo.','Missing software')
   return
end

% choose an altitude
alt  = 600;

% mean orbit rate
Re   = 6378.14;
sma  = Re + alt;
n    = OrbRate( sma );

% open-loop plant
[A,B] = LinOrb( [], n );
nI    = size(B,2);
nS    = size(A,1);
C     = eye(nS);
D     = zeros(nS,nI);

w.qE = eye(6);    % State noise input matrix
w.rE = eye(6);    % Measurement noise covariance matrix
w.q  = (1e-4)*C'*C;    % output weighting matrix
w.r  = (1e+4)*eye(3);  % input weighting matrix

KR   = QCR( A, B, w.q, w.r );    % regulator (gain matrix)
KE   = QCE( A, C, w.qE, w.rE );  % estimator (gain matrix)

Combine the estimator and regulator into one controller

%--------------------------------------------------------
[aK,bK,cK,dK] = LinOrbLQG([sma,0,0,0,0,0],1e-4,1e4);

% systems
p = pck(A,B,C,D);            % open-loop plant
k = pck(aK,bK,cK,dK);        % controller

% interconnected system
systemnames  = ' p k ';
inputvar     = '[ r(6) ]';
outputvar    = '[ p ; k ]';
input_to_p   = '[ k ]';
input_to_k   = '[ r ; p ]';
sysoutname   = 'clp';
cleanupsysic = 'yes';
sysic;

% initial & final state
x0 = zeros(6,1);
xF = [0;1;0;0;0;0];

% generate a trajectory to follow
T    = Period(sma);
dTLP = 10;
dur  = round(T/dTLP)*dTLP;
t1   = 0:dTLP:dur*2;
aC   = LPCircular(x0, xF, n, dur, dTLP );
aC   = [aC, zeros(3,length(t1)-size(aC,2)-1)];
xS   = DiscreteHills( x0, n, aC, dTLP );
r1   = xS;

% choose a time-step for discretizing the system
% and recompute the trajectory (linear interpolation btwn points)
dT   = 1;
t2   = 0:dT:dur*2;
r    = interp1( t1, r1', t2 )';

% closed-loop system response
tnew  = 0:0.05:t2(end);
rnew  = interp1( t2, r', tnew )';
ref   = vpck( rnew(:), tnew );
yCL   = trsp(clp,ref,t2(end),.05,[xF;xF]*0);

figure, vplot(sel(yCL,1:3,1)), title('position');
figure, vplot(sel(yCL,4:6,1)), title('velocity');
figure, vplot(sel(yCL,7:9,1)), title('control');


%--------------------------------------
% PSS internal file version information
%--------------------------------------

Relative orbit control using LQG

Contents

Combine the estimator and regulator into one controller