Skip to content

Observability: staircase

12/01/2012
% compute the staircase form for observability analysis
clc;clear;
e1=[1; 0; 0];
e2=[0; 1; 0];
e3=[0; 0; 1];

vn=[0;0;0];
vn_sk=fcn_getSkew(vn);

a=-10*e3;
a_sk=fcn_getSkew(a);

A=[zeros(3)   eye(3)     zeros(3)   zeros(3)    zeros(3);
     zeros(3)   zeros(3)   -a_sk      eye(3)       zeros(3);
     zeros(3)   zeros(3)   zeros(3)   zeros(3)   eye(3);
     zeros(3)   zeros(3)   zeros(3)   zeros(3)   zeros(3);
     zeros(3)   zeros(3)   zeros(3)   zeros(3)   zeros(3)];
 
C=[zeros(3)   zeros(3)   -vn*e2'     zeros(3)   zeros(3);
     zeros(3)   zeros(3)   vn*e1'      zeros(3)   zeros(3);
     zeros(3)   eye(3)     -vn_sk      zeros(3)   zeros(3)];
 
 B=zeros(15,1);
 
 [Abar,Bbar,Cbar,T,k] = obsvf(A,B,C) ;
 
 % use symbolic computation
 syms px py pz vx vy vz phi tht psi bax bay baz bp bq br X
 % X is the oringinal state vector
 X=[px; py; pz; vx; vy; vz; phi; tht; psi; bax; bay; baz; bp; bq; br];
 % TX is the transformed state vector, it is clear to find what states are
 % observable
 T*X
Advertisements

Comments are closed.

%d bloggers like this: