diff options
Diffstat (limited to '')
| -rw-r--r-- | pbhtest.m | 58 | ||||
| -rw-r--r-- | uav_model.m | 230 |
2 files changed, 132 insertions, 156 deletions
diff --git a/pbhtest.m b/pbhtest.m new file mode 100644 index 0000000..f56aa21 --- /dev/null +++ b/pbhtest.m @@ -0,0 +1,58 @@ +% Do A PBH test on system. Given SYS returns: +% +% - Nr. of states +% - Nr. of stable states +% - Nr. of controllable states +% - Nr. of unstabilizable states +% - Nr. of observable states +% - Nr. of undetectable states + +function [nx, nsta, nctrb, nustab, nobsv, nudetb] = pbhtest(sys) + +nx = length(sys.A); +eigvals = eig(sys.A); + +% Count number of stable states +nsta = 0; +for i = 1:nx + if real(eigvals(i)) < 0 + nsta = nsta + 1; + end +end + +% Check system controllability / stabilizability +Wc = ctrb(sys); +nctrb = rank(Wc); +if nctrb < nx + % Is the system at least stabilizable? + nustab = 0; + for i = 1:nx + if real(eigvals(i)) >= 0 + % PBH test + W = [(sys.A - eigvals(i) * eye(nx)), sys.B]; + if rank(W) < nx + nustab = nustab + 1; + end + end + end +end + +% Check system observability / detectability +Wo = obsv(sys); +nobsv = rank(Wo); +if nobsv < nx + % is the system at least detectable? + nudetb = 0; + for i = 1:nx + if real(eigvals(i)) >= 0 + % PBH test + W = [sys.C; (sys.A - eigvals(i) * eye(nx))]; + if rank(W) < nx + nudetb = nudetb + 1; + end + end + end +end + +end + diff --git a/uav_model.m b/uav_model.m index fda2745..6a7f9da 100644 --- a/uav_model.m +++ b/uav_model.m @@ -285,74 +285,19 @@ model.linear = struct(... % ------------------------------------------------------------------------ % Check properties of linearized model -eigvals = eig(sys.A); - -% Check system controllability / stabilizability -Wc = ctrb(sys); -if rank(Wc) < nx - fprintf(' - Linearized system has %d uncontrollable states!\n', ... - (nx - rank(Wc))); - - % Is the system at least stabilizable? - unstabilizable = 0; - for i = 1:nx - if real(eigvals(i)) >= 0 - % PBH test - W = [(sys.A - eigvals(i) * eye(nx)), sys.B]; - if rank(W) < nx - % fprintf(' State %d is not stabilizable\n', i); - unstabilizable = unstabilizable + 1; - end - end - end - if unstabilizable > 0 - fprintf(' - Linearized system has %d unstabilizable modes!\n', ... - unstabilizable); - else - fprintf(' However, it is stabilizable.\n'); - end -end +[nx, nsta, nctrb, nustab, nobsv, nudetb] = pbhtest(sys); -% Check system observability / detectability -Wo = obsv(sys); -if rank(Wo) < nx - fprintf(' - Linearized system has %d unobservable states!\n', ... - (nx - rank(Wo))); - % is the system at least detectable? - undetectable = 0; - for i = 1:nx - if real(eigvals(i)) >= 0 - % PBH test - W = [sys.C; (sys.A - eigvals(i) * eye(nx))]; - if rank(W) < nx - undetectable = undetectable + 1; - end - end - end - if undetectable > 0 - fprintf(' - Linearized system has %d undetectable modes!\n', ... - undetectable); - else - fprintf(' However, it is detectable.\n') - end -end +fprintf(' - Linearized system has %d states:\n', nx); +fprintf(' %d stable modes, %d unstable modes.\n', nsta, nx - nsta); +fprintf(' %d controllable modes, %d unstabilizable modes.\n', nctrb, nustab); +fprintf(' %d observable modes, %d undetectable modes.\n', nobsv, nustab); -stable = 0; -unstable = 0; -for i = 1:nx - if real(eigvals(i)) < 0 - stable = stable + 1; - else - unstable = unstable + 1; - end +if nctrb < 12 + error('Linearized model has less than 12 controllable modes!'); end -fprintf(' - Linearized system has %d states:\n', nx); -fprintf(' %d controllable modes, %d observable modes.\n', rank(Wc), rank(Wo)); -fprintf(' %d stable modes, %d unstable modes.\n', stable, unstable); - -if rank(Wc) < 12 - error('Uncertain model has less than 12 controllable modes'); +if nustab > 0 || nudetb > 0 + error('Linearized model has unstabilizable or undetectable modes!'); end % ------------------------------------------------------------------------ @@ -455,109 +400,82 @@ model.uncertain.Ny = max(size(idx.OutputNominal)); % ------------------------------------------------------------------------ % Check properties of uncertain model -eigvals = eig(usys.A); -nx = size(usys.A, 1); - -% Check system controllability / stabilizability -Wc = ctrb(usys); -if rank(Wc) < nx - fprintf(' - Uncertain system has %d uncontrollable states!\n', ... - (nx - rank(Wc))); - - % Is the system at least stabilizable? - unstabilizable = 0; - for i = 1:nx - if real(eigvals(i)) >= 0 - % PBH test - W = [(usys.A - eigvals(i) * eye(nx)), usys.B]; - if rank(W) < nx - % fprintf(' State %d is not stabilizable\n', i); - unstabilizable = unstabilizable + 1; - end - end - end - if unstabilizable > 0 - fprintf(' - Uncertain system has %d unstabilizable modes!\n', ... - unstabilizable); - else - fprintf(' However, it is stabilizable.\n'); - end -end +[nx, nsta, nctrb, nustab, nobsv, nudetb] = pbhtest(usys); + +fprintf(' - Uncertain system has %d states:\n', nx); +fprintf(' %d stable modes, %d unstable modes.\n', nsta, nx - nsta); +fprintf(' %d controllable modes, %d unstabilizable modes.\n', nctrb, nustab); +fprintf(' %d observable modes, %d undetectable modes.\n', nobsv, nustab); -% Check system observability / detectability -Wo = obsv(usys); -if rank(Wo) < nx - fprintf(' - Uncertain system has %d unobservable states!\n', ... - (nx - rank(Wo))); - % is the system at least detectable? - undetectable = 0; - for i = 1:nx - if real(eigvals(i)) >= 0 - % PBH test - W = [usys.C; (usys.A - eigvals(i) * eye(nx))]; - if rank(W) < nx - undetectable = undetectable + 1; - end - end - end - if undetectable > 0 - fprintf(' - Uncertain system has %d undetectable modes!\n', ... - undetectable); - else - fprintf(' However, it is detectable.\n') - end +if nctrb < 12 + error('Uncertain model has less than 12 controllable modes!'); end -stable = 0; -unstable = 0; -for i = 1:nx - if real(eigvals(i)) < 0 - stable = stable + 1; - else - unstable = unstable + 1; - end +if nustab > 0 || nudetb > 0 + error('Uncertain model has unstabilizable or undetectable modes!'); end -fprintf(' - Uncertain system has %d states:\n', nx); -fprintf(' %d controllable modes, %d observable modes.\n', rank(Wc), rank(Wo)); -fprintf(' %d stable modes, %d unstable modes.\n', stable, unstable); % % Check that (A, B_u, C_y) is stabilizable and detectable -% A = model.uncertain.StateSpace(... -% model.uncertain.index.OutputUncertain, ... -% model.uncertain.index.InputUncertain ... -% ); -% -% B_u = model.uncertain.StateSpace(... -% model.uncertain.index.OutputUncertain, ... -% model.uncertain.index.InputNominal ... -% ); -% -% % TODO: do PBH test -% -% % Check that D_eu and D_yw are full rank -% D_eu = model.uncertain.StateSpace(... -% model.uncertain.index.OutputError, ... -% model.uncertain.index.InputNominal ... -% ); -% -% D_yw = model.uncertain.StateSpace(... -% model.uncertain.index.OutputNominal, ... -% model.uncertain.index.InputDisturbance ... -% ); - -% if rank(D_eu) < min(size(D_eu)) -% fprintf('D_eu is not full rank!\n') -% end -% -% if rank(D_yw) < min(size(D_yw)) -% fprintf('D_yw is not full rank!\n') -% end +A = model.uncertain.StateSpace(... + model.uncertain.index.OutputUncertain, ... + model.uncertain.index.InputUncertain ... +); + +[~, ~, ~, nustab, ~, nudetb] = pbhtest(A); + +if nustab > 0 || nudetb > 0 + fprintf(' Uncertain system has undetectable or uncontrollable A.\n'); +end + +B_u = model.uncertain.StateSpace(... + model.uncertain.index.OutputUncertain, ... + model.uncertain.index.InputNominal ... +); + +[~, ~, ~, nustab, ~, nudetb] = pbhtest(B_u); + +if nustab > 0 || nudetb > 0 + fprintf(' Uncertain system has undetectable or uncontrollable Bu.\n'); +end + +C_y = model.uncertain.StateSpace(... + model.uncertain.index.OutputNominal, ... + model.uncertain.index.InputUncertain ... +); + +[~, ~, ~, nustab, ~, nudetb] = pbhtest(C_y); + +if nustab > 0 || nudetb > 0 + fprintf(' Uncertain system has undetectable or uncontrollable Cy.\n'); +end + + +% Check that D_eu and D_yw are full rank +D_eu = model.uncertain.StateSpace(... + model.uncertain.index.OutputError, ... + model.uncertain.index.InputNominal ... +); + +D_yw = model.uncertain.StateSpace(... + model.uncertain.index.OutputNominal, ... + model.uncertain.index.InputDisturbance ... +); + +if rank(ctrb(D_eu)) < length(D_eu.A) || rank(obsv(D_eu)) < length(D_eu.A) + fprintf(' D_eu is not full rank!\n') +end + +if rank(ctrb(D_yw)) < length(D_yw.A) || rank(obsv(D_yw)) < length(D_yw.A) + fprintf(' D_yw is not full rank!\n') +end + +% TODO: column rank checks end function [indices] = make_idx(start, size) - indices = [start:(start + size - 1)]'; + indices = (start:(start + size - 1))'; end % vim: ts=2 sw=2 et: |