% satellite attitude control example
clear;
close all;
% We're not concerned with actual values: masses, frequency or time
% response specifics, thrust limits, etc.
%
% Model the plant simply. Here assume rotational moment of intetia is 1
J=1;
% double integrator plant: From textbook example this is the 1/s^2 plant
nump=1; % plant model numerator
denp=[J 0 0]; % plant model denominator
% lead compensator
% Our simple plant model has a "flat" phase at -180. An actual plant
% will have additional actuator poles that limit where we can place the
% lead zero, but not for our simple simulation model. Pick some convenient
% values for the lead zero and cancelling pole.
lz=1; % lead zero
lp=10; % lead-cancelling pole
num = [1 lz]; % lead-compensated plant numerator
den = conv([1 lp],[denp]); % demonominator
% establish required gain for unity-gain crossover at max phase margin
[mag, ph,W] = bode(num,den);
[maxph, idx] = max(ph); % get the bode plot data phase peak
fcross = W(idx)/(2*pi);
Kp = 1/mag(idx); % gain required for unity gain crossover here.
% resultant compensated system model: apply the gain to the numerator.
num = Kp*num;
% Convert to state-space for the ordinary differential equation solver
% Matlab supplies (ode45 below)
[A,B,C,D] = tf2ss(num,den);
% now let's experiment with time-domain simulation techniques...
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% we'll capture the simulation in a video so initialize the file
vidfile = VideoWriter('satellite_attitude_sim.avi');
open(vidfile);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initialize a capsule graphic...
% This is a bullet-shaped object that will animate the rotation
fig = figure('Name','Satellite attitude');
set(fig, 'Position', [10 10 1024 768])
subplot(2,3,1)
color = get(fig,'Color');
set(gca,'XColor',color,'YColor',color,'TickDir','out')
% verticies of the graphic
v = [1 -2; 1 2; 0 3.5; -1 2; -1 -2; 1 -2]
%initial position
startangle = 0; %-pi/2;
R = [cos(startangle) -sin(startangle); sin(startangle) cos(startangle)];
v = v*R;
% this is the thruster graphic
tlen=2
th = [0 0; tlen 0]
thr = th*R + [v(1,:); v(1,:)];
hPlot = plot(v(:,1),v(:,2), 'b-',0,0,'r-');
title('Capsule with Thrusters');
axis([-4 4 -4 4])
grid on
set(hPlot,{'LineWidth'},{2;10})
xlim([-4,4])
ylim([-4,4])
set(hPlot(1),'XData',[v(:,1)],'YData',[v(:,2)]);
MM = getframe(fig);
writeVideo(vidfile,MM);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% initialize the sim
ic = [0 0 0]; % initial conditions: zero rotational accel, vel, pos.
xout=[0 0 0]; % initialize the state output matrix
tend = 4; % simulation end time
refin = pi; % angle reference we're going to drive the capsule to
% tstep is the outer loop time-step below. This is our, "sampling rate".
% This is a continuous-time model to keep it simple, but you'll see below
% that the ODE solver is called in a loop with this as the time interval
% between iterations. If you make this time longer you will see the
% deliterious effect of slow sampling rate on plant dynamics
% because the solver is computing plant dynamics between iterations.
% but we're, "sampling" the angle and updating the output too slowly.
%
% This is, "aliasing" relative to the controller-plant bandwidth that
% is nominally the open-loop unity-gain crossover frequency we get from
% the Bode plot data above.
%
% Go ahead an play with this and you'll see dynamic performace get worse
% and eventually unstable.
%
% Here I pick a, "sampling rate" that is 50-times faster than the
% controller-plant model. This should nearly negate the effect
% of sampling rate on the plant, meaning the continuous-time model
% is valid: you shouldn't see the effect of sampling rate on the state
% dynamics plot. Slow this down (change 0.02 to 0.1, 0.5, etc.) and
% you will see the effect of sampling rate on continuous model dynamics.
tstep = .02/fcross;
% These are some variables the loop will populate for plotting
tout=0; % time output starting point
eout=0; % error output init
refout=0;
tm=0;
yout = 0;
yzohout = 0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% initialize additional data plots
% Output Angle
subplot(2,3,2)
hPlotYout = plot(0,0,'r-',0,0,'b:',0,0,'k-');
legend('actual','desired','sampled');
title('Capsule Position');
ylabel('degrees');
xlabel('time(s)');
xlim([0,tend])
% command input
subplot(2,3,3)
hPlotErr = plot(0,0,'r-');
title('Sampled Position Angle Error');
ylabel('Degrees');
xlabel('time(s)');
xlim([0,tend])
% bode plot
subplot(2,3,5)
bode(num,den);
grid on
% Root Locus for gain=1 up to our controller gain
subplot(2,3,6)
rlocus(num,den,[1:.001:Kp]);
grid on
% This just holds the capsule graphic still for a second on-screen
% before the controller is unleashed.
pause(1)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Control Loop Simulation
% loop until the specified simulation end time
% tm will increase by tstep before looping-back
while tm < tend,
% each iteration will run the ODE solver for a timespan of tstep.
% split this into ten increments for the solver. In this way the
% solver is less discretized that our control loop, by a factor of
% 10 here. This can be thought of as simulating the, "real" plant
% as a closer-to-continuous model as we sample the position and
% adjust the thrusters at a slower rate.
% the solver starts at zero-time each step. This doesn't matter so
% long as the initial conditions are supplied correctly, which you
% will see below.
tspan = 0:tstep/10:tstep;
err = (refin - yout); % output error relative to reference input
% pass this as command input to the solver. The cotroller gain is
% applied within the solver
u = err;
% invoke the ODE solver to compute state variables for each interval of
% tspan. "rpcont" if where the state-space model lives so examine that.
[t,x] = ode45(@(t,x) ssodefun(t,x,A,B,C,u), tspan, ic);
% post-solver iteration data collection
% the solver output time vector is zero-based. Offset by iteration
% start time
tt = tm+t;
tout = [tout; tt]; % append this iteration time vector to total sim time
xout = [xout; x]; % append iteration states to sim state data
sz = size(x); % size of the state vector
% Initial conditions for next iteration are the states of the last
% output of the solver for this iteration
ic = x(sz(1),:);
% error for entire iteration interval is constant because we only
% "sample" it at the iteration loop rate
et(1:sz(1)) = err;
eout = [eout et]; % append the error vector for this iteration.
% Same for the reference input
rt(1:sz(1)) = refin;
refout = [refout rt];
% Apply the state-space 'C' matrix to the state matrix to obtain
% angular output for this iteration
yo = C*x';
% the "sampled" output for the next iteration is that last
% position solved in this iteration.
yout = yo(sz(1));
% the "sampled" position is the last value for this iteration
yzoh(1:sz(1)) = yo(sz(1));
yzohout = [yzohout yzoh];
R = [cos(yout) -sin(yout); sin(yout) cos(yout)];
vo = v*R;
% scale the thruster with the error
thruster_scale = 4;
th = [0 0; err*thruster_scale 0]
% paint the thruster out the proper side of the capsule...
% rotate the trust vector and offset it to the proper corner of the
% capsule.
if err>0,
thr = th*R + [vo(1,:); vo(1,:)];
else
thr = th*R + [vo(5,:); vo(5,:)];
end
% set the capsule graphic plot data for this iteration
set(hPlot(1),'XData',vo(:,1),'YData',vo(:,2));
set(hPlot(2),'XData',thr(:,1),'YData',thr(:,2));
% paint the capsule position and reference plot
yy = C*xout'
set(hPlotYout(1),'XData',tout,'YData',yy*180/pi);
set(hPlotYout(2),'XData',tout,'YData',refout*180/pi);
set(hPlotYout(3),'XData',tout,'YData',yzohout*180/pi);
% paint the current error plot
set(hPlotErr,'XData',tout,'YData',eout*180/pi);
% set the start time tm for the next iteraton.
tm = tm + tstep;
% write the figure frame to the video file
MM = getframe(fig);
writeVideo(vidfile,MM);
% loop back for the next iteration
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% simulation complete
close(vidfile);