echo off
%	balred.m	Balanced modred reduction of actuator/suspension model
%	from act8.m.
	
	clear all;

	hold off;

	clf;

	load  act8_data;

%	plot dc gain and two contibutors, force and xn, versus mode

	index_states = 1:num_modes_total-1;

   	omega1 = 2*pi*freqvec';     %  convert to radians

   	semilogy(index_orig(2:num_modes_total)-1,gain_h0(2:num_modes_total),'k.-', ...
			 index_orig(2:num_modes_total)-1,abs(force(2:num_modes_total)./ ...
			 						omega1(2:num_modes_total)),'k-', ...
			 index_orig(2:num_modes_total)-1, ...
	 		 abs(xn(8,2:num_modes_total)./omega1(2:num_modes_total)),'ko-')
   	title([headstr ' dc gain, force and xn values versus mode number'])
   	xlabel('mode number')
   	ylabel('dc value')
	legend('dc gain','force','xn',3)
   	grid off

	disp('execution paused to display figure, "enter" to continue'); pause

%	define oscillatory system from unsorted model from act8.m, which only has one output,
%	either head 0 or head 1 so that when use balreal, will only be taking into account
%   a siso system, not the outputs of both heads 0 and 1

%	in act8.m, used output matrix with two rows so both head 0 and head 1 were available

	a_syso = a(3:asize,3:asize);		% ao is a for oscillatory system

	b_syso = b(3:asize);

	c_syso = c_disp(index_out+6,3:asize);

	syso = ss(a_syso,b_syso,c_syso,d);

%	define controllability and observability gramians for oscillatory system, syso

	wc = gram(syso,'c');

	wo = gram(syso,'o');

	[row_syso,col_syso] = size(a_syso);

	statevec = 1:row_syso;

%	calculate closed form gramians

%	define frequencies for oscillatory states

   	omega1 = 2*pi*freqvec';     %  convert to radians

	ctr = 0;

	for  cnt = 1:num_modes_total

		ctr = ctr + 2;

		omega12(ctr-1) = omega1(cnt);

		omega12(ctr) = omega1(cnt);

		zeta_unsort12(ctr-1) = zeta_unsort(cnt);

		zeta_unsort12(ctr) = zeta_unsort(cnt);

	end

%	the notation below is "wc" or "wo" for controllability or observability gramians,
%	"cf" for closed-form, and "1" or "2" for maximum and minimum values for a mode

	wccf1 = (b_syso.*b_syso)./(4*zeta_unsort12(3:2*num_modes_total)'.*omega12(3:2*num_modes_total)');		% maximum terms

	wccf12 = wccf1(2:2:row_syso);		% pick out velocity terms

	wccf2 = (b_syso.*b_syso)./(4*zeta_unsort12(3:2*num_modes_total)'.*omega12(3:2*num_modes_total)'.^3);	% minimum terms
	
	wccf22 = wccf2(2:2:row_syso);		% pick out displacement terms

	wocf1 = (c_syso.*c_syso)./(4*zeta_unsort12(3:2*num_modes_total).*omega12(3:2*num_modes_total));		% maximum terms

	wocf12 = wocf1(1:2:row_syso);		% pick out displacement terms

	wocf2 = (c_syso.*c_syso)./(4*zeta_unsort12(3:2*num_modes_total).*omega12(3:2*num_modes_total).^3);		% minimum terms

	wocf22 = wocf2(1:2:row_syso);		% pick out velocity terms

%	plot controllability and observability gramians

	meshz(wc);
	view(60,30);
	title([headstr ', controllability gramian for oscillatory system'])
	xlabel('state')
	ylabel('state')
	grid on

	disp('execution paused to display figure, "enter" to continue'); pause

	meshz(wo);
	view(60,30);
	title([headstr ', observability gramian for oscillatory system'])
	xlabel('state')
	ylabel('state')
	grid on

	disp('execution paused to display figure, "enter" to continue'); pause

%	pull out diagonal elements

	wc_diag = diag(wc);

	wo_diag = diag(wo);

	modevec = 2*(1:num_modes_total-1);

%	plot diagonal terms of controllability and observability gramians, calculated with
%	gram function and closed form

	semilogy(statevec,wc_diag,'k.-',statevec(2:2:row_syso),wccf12,'ko', ...
						    	    statevec(1:2:row_syso),wccf22,'ko')
	title([headstr ', controllability gramian diagonal terms'])
	xlabel('states')
	ylabel('diagonal')
	legend('calculated with gram','closed form',3)
	grid off

	disp('execution paused to display figure, "enter" to continue'); pause

	semilogy(statevec,wo_diag,'k.-',statevec(1:2:row_syso),wocf12,'ko', ...
						    	    statevec(2:2:row_syso),wocf22,'ko')
	title([headstr ', observability gramian diagonal terms'])
	xlabel('states')
	ylabel('diagonal')
	legend('calculated with gram','closed form',3)
	grid off

	disp('execution paused to display figure, "enter" to continue'); pause

%	position and velocity states plotted separately

	semilogy(statevec(1:2:row_syso),wc_diag(1:2:row_syso),'k.-', ...
		 					statevec(2:2:row_syso),wc_diag(2:2:row_syso),'k-', ...
						 	statevec(2:2:row_syso),wccf12,'ko', ...
						    statevec(1:2:row_syso),wccf22,'ko')
	title([headstr ', controllability gramian diagonal terms'])
	xlabel('states')
	ylabel('diagonal')
	legend('position states','velocity states','closed form','closed form',3)
	grid off

	disp('execution paused to display figure, "enter" to continue'); pause

	semilogy(statevec(1:2:row_syso),wo_diag(1:2:row_syso),'k.-', ...
		  	 				statevec(2:2:row_syso),wo_diag(2:2:row_syso),'k-', ...
							statevec(1:2:row_syso),wocf12,'ko', ...
						    statevec(2:2:row_syso),wocf22,'ko')
	title([headstr ', observability gramian diagonal terms'])
	xlabel('states')
	ylabel('diagonal')
	legend('position states','velocity states','closed form','closed form',3)
	grid off

	disp('execution paused to display figure, "enter" to continue'); pause

   	semilogy(index_states,wc_diag(2:2:row_syso),'k.-', ...
   			index_states,wo_diag(1:2:row_syso),'ko-')
   	title([headstr ', head 0 controllability and observability state gramians'])
   	xlabel('mode number')
   	ylabel('gramian')
	legend('controllability velocity state','observability position state',3)
   	grid off

	disp('execution paused to display figure, "enter" to continue'); pause

%	use balreal to rank oscillatory states and modred to reduce for comparison

	[sysob,g,T,Ti] = balreal(syso);

%	define controllability and observability gramians for balanced oscillatory system, sysob

	wcb = gram(sysob,'c');

	wob = gram(sysob,'o');

	wcb_diag = diag(wcb);

	wob_diag = diag(wob);

	modevec = 2*(1:num_modes_total-1);

%	plot balanced controllability and observability gramians

	meshz(wcb);
	view(60,30);
	title([headstr ', oscillatory system balanced controllability gramian'])
	xlabel('state')
	ylabel('state')
	grid on

	disp('execution paused to display figure, "enter" to continue'); pause

	meshz(wob);
	view(60,30);
	title([headstr ', oscillatory system balanced observability gramian'])
	xlabel('state')
	ylabel('state')
	grid on

	disp('execution paused to display figure, "enter" to continue'); pause

%	plot diagonal terms of balanced controllability and observability gramians

	semilogy(statevec,wcb_diag,'k.-',statevec,wob_diag,'ko-')
	title([headstr ', balanced system controllability and observability gramian diagonal terms'])
	xlabel('states')
	ylabel('diagonal')
	legend('controllability','observability',3)
	grid off

	disp('execution paused to display figure, "enter" to continue'); pause

%	plot sorted diagonal values and dc gain	

	[row_syso,col_syso] = size(a_syso);

	semilogy(statevec,g,'k.-',2*index_orig((2:num_modes_total)-1), ...
						gain_h0_sort(2:num_modes_total),'k-')
	title([headstr ', sorted diagonal terms of balanced gramian and dc gain'])
	xlabel('state')
	ylabel('diagonal of gramian')
	legend('balanced','dc gain',3)
	grid off

	disp('execution paused to display figure, "enter" to continue'); pause

	num_oscil_states_used = 2*num_modes_used - 2;

%	use modred to reduce states from balanced system using both "del" and "mdc"

	bsys_delo = modred(sysob,num_oscil_states_used+1:2*num_modes_total-2,'del');

	bsys_mdco = modred(sysob,num_oscil_states_used+1:2*num_modes_total-2,'mdc');

%	rebuild system by appending balanced realization of oscillatory modes to rigid body mode

	[a_delo_bal,b_delo_bal,c_delo_bal,d_delo_bal] = ssdata(bsys_delo);

	a_del_bal = [   a(1:2,1:2)      zeros(2,num_oscil_states_used)
			     zeros(num_oscil_states_used,2)      a_delo_bal    ];
		
	b_del_bal = [b(1:2,:)
			     b_delo_bal];

	c_del_bal = [c_disp(index_out+6,1:2) c_delo_bal];

	bsys_del = ss(a_del_bal,b_del_bal,c_del_bal,d);

	[a_mdco_bal,b_mdco_bal,c_mdco_bal,d_mdco_bal] = ssdata(bsys_mdco);

	a_mdc_bal = [   a(1:2,1:2)      zeros(2,num_oscil_states_used)
			     zeros(num_oscil_states_used,2)      a_mdco_bal    ];
		
	b_mdc_bal = [b(1:2,:)
			     b_mdco_bal];

	c_mdc_bal = [c_disp(index_out+6,1:2) c_mdco_bal];

	bsys_mdc = ss(a_mdc_bal,b_mdc_bal,c_mdc_bal,d);

	[magr_del,phsr_del] = bode(bsys_del,frad);

	[magr_mdc,phsr_mdc] = bode(bsys_mdc,frad);

%	compare frequency responses for all four reduction methods

	loglog(f,mag(index_out,:),'k--',f,mag_sort_red(index_out,:),'k-', ...
								f,magr_del(1,:),'k.-')
	title([headstr ', results comparison, ',num2str(num_modes_used),' modes, ', ...
					num2str(num_oscil_states_used),' oscillatory balanced states'])
   	xlabel('Frequency, hz')
	ylabel('Magnitude, mm')
	axis([500 25000 1e-8 1e-4])
	legend('all modes','sorted truncated','balreal modred del',3)
   	grid off

	disp('execution paused to display figure, "enter" to continue'); pause

	loglog(f,mag(index_out,:),'k--',f,mag_mdc(index_out,:),'k-',f,magr_mdc(1,:),'k.-')
	title([headstr ', results comparison, ',num2str(num_modes_used),' modes, ', ...
					num2str(num_oscil_states_used),' oscillatory balanced states'])
   	xlabel('Frequency, hz')
	ylabel('Magnitude, mm')
	axis([500 25000 1e-8 1e-4])
	legend('all modes','sorted mdc','balreal modred mdc',3)
   	grid off

	disp('execution paused to display figure, "enter" to continue'); pause

%	calculate impulse responses of all four oscillatory systems for comparison

	ttotal = 0.0025;

	t = linspace(0,ttotal,400);

%	define oscillatory systems for models

%	sorted reduced system

	red_size = 2*num_modes_used;

	[a_sys_sort_red,b_sys_sort_red,c_sys_sort_red,d_sys_sort_red] = ssdata(sys_sort_red);	

	a_sys_sort_redo = a_sys_sort_red(3:red_size,3:red_size);

	b_sys_sort_redo = b_sys_sort_red(3:red_size);

	c_sys_sort_redo = c_sys_sort_red(index_out,3:red_size);

	sys_sort_redo = ss(a_sys_sort_redo,b_sys_sort_redo,c_sys_sort_redo,d);

%	sorted mdc reduced system

	[a_sys_sort_mdc,b_sys_sort_mdc,c_sys_sort_mdc,d_sys_sort_mdc] = ssdata(sys_mdc);	

	a_sys_sort_mdc = a_sys_sort_red(3:red_size,3:red_size);

	b_sys_sort_mdc = b_sys_sort_red(3:red_size);

	c_sys_sort_mdc = c_sys_sort_red(index_out,3:red_size);

	sys_mdco = ss(a_sys_sort_mdc,b_sys_sort_mdc,c_sys_sort_mdc,d);

%	use lsim to calculate transient response

	[disp_syso,t_syso] = impulse(syso,t);

	[disp_sys_sort_redo,t_sys_sort_redo] = impulse(sys_sort_redo,t);

	[disp_sys_sort_mdco,t_sys_sort_mdco] = impulse(sys_mdco,t);

	[disp_bsys_delo,t_bsys_delo] = impulse(bsys_delo,t);

	[disp_bsys_mdco,t_bsys_mdco] = impulse(bsys_mdco,t);

%	build matrix of results

	dispo = [disp_syso(:,1) disp_sys_sort_redo(:,1) ...
		    disp_sys_sort_mdco(:,1) disp_bsys_delo(:,1) ...
			disp_bsys_mdco(:,1)];

	sort_redo_del = dispo(:,1) - dispo(:,2);

	sort_mdco_del = dispo(:,1) - dispo(:,3);

	delo_del = 		dispo(:,1) - dispo(:,4);

	mdco_del = 		dispo(:,1) - dispo(:,5);

%	calculate normalized reduction index

	index_sort_redo = sqrt(sum(sort_redo_del.*sort_redo_del))/sqrt(sum(dispo(:,1).*dispo(:,1)))

	index_sort_mdco = sqrt(sum(sort_mdco_del.*sort_mdco_del))/sqrt(sum(dispo(:,1).*dispo(:,1)))

	index_delo = 	  sqrt(sum(delo_del.*delo_del))/sqrt(sum(dispo(:,1).*dispo(:,1)))

	index_mdco = 	  sqrt(sum(mdco_del.*mdco_del))/sqrt(sum(dispo(:,1).*dispo(:,1)))

	[num_modes_used index_sort_redo index_sort_mdco index_delo index_mdco]

	plot(t_syso,disp_syso(:,1),'k-',t_sys_sort_redo,disp_sys_sort_redo(:,1),'k.-')
	title([headstr ', displacement vs time, ',num2str(num_modes_used-1),' oscillatory modes'])
	xlabel('time, sec')
	ylabel('displacement, mm')
	legend('all modes','sorted reduced system',4)
	grid off

	disp('execution paused to display figure, "enter" to continue'); pause

	plot(t_syso,disp_syso(:,1),'k-',t_sys_sort_mdco,disp_sys_sort_mdco(:,1),'k.-')
	title([headstr ', displacement vs time, ',num2str(num_modes_used-1),' oscillatory modes'])
	xlabel('time, sec')
	ylabel('displacement, mm')
	legend('all modes','sorted modred mdc',4)
	grid off

	disp('execution paused to display figure, "enter" to continue'); pause

	plot(t_syso,disp_syso(:,1),'k-',t_bsys_delo,disp_bsys_delo(:,1),'k.-')
	title([headstr ', displacement vs time, ',num2str(num_oscil_states_used),' oscillatory balanced states'])
	xlabel('time, sec')
	ylabel('displacement, mm')
	legend('all modes','balreal modred del',4)
	grid off

	disp('execution paused to display figure, "enter" to continue'); pause

	plot(t_syso,disp_syso(:,1),'k-',t_bsys_mdco,disp_bsys_mdco(:,1),'k.-')
	title([headstr ', displacement vs time, ',num2str(num_oscil_states_used),' oscillatory balanced states'])
	xlabel('time, sec')
	ylabel('displacement, mm')
	legend('all modes','balreal modred mdc',4)
	grid off

	disp('execution paused to display figure, "enter" to continue'); pause

%	plot results of oscillatory impulse response normalized error index versus
%	number of modes used	
	
	error_norm = [	2	.4332	.4332  0.3007    0.3008
					3	.3041	.3041  0.1777    0.1823
					4	.1759	.1759  0.1135    0.1137
					5	.1134	.1134  0.0845    0.0841
					6	.0851	.0851  0.0598    0.0603
					7	.0637	.0637  0.0582    0.0583
					8	.0599	.0599  0.0383    0.0401
					9	.0594	.0594  0.0343    0.0356
				   10   .0572   .0572  0.0338    0.0347
				   11   .0555   .0555  0.0258    0.0264
				   12   .0392   .0392  0.0280    0.0268
				   13   .0327   .0327  0.0167    0.0168
				   14   .0270   .0270  0.0162    0.0158
				   15   .0209   .0209  0.0162    0.0156];	

	nmode = error_norm(:,1);
	
	error_sort_red = error_norm(:,2);
	
	error_sort_mdc = error_norm(:,3);						
	
	error_bal_del = error_norm(:,4);						
	
	error_bal_mdc = error_norm(:,5);						

	plot(nmode,error_sort_red,'k.-',nmode,error_bal_del,'ko-')
	title([headstr ', normalized reduction index versus number of modes included'])
	xlabel('number of modes included')
	ylabel('normalized reduction index')
	legend('sorted reduced','balanced del')
	axis([0 15 0 0.5])
	grid off
	
	disp('execution paused to display figure, "enter" to continue'); pause

	plot(nmode,error_sort_mdc,'k.-',nmode,error_bal_mdc,'ko-')
	title([headstr ', normalized reduction index versus number of modes included'])
	xlabel('number of modes included')
	ylabel('normalized reduction index')
	legend('sorted mdc','balanced mdc')
	axis([0 15 0 0.5])
	grid off
	
	disp('execution paused to display figure, "enter" to continue'); pause

	save  balred_data;