📄 stat_up.m
字号:
function [sys,x0,str,ts]=quat_upd(t,x,u,flag,dt,tiltxi,tiltyi,tiltzi,gbxi,gbyi,gbzi,gsfxi,gsfyi,gsfzi,dx,dy,dz)
%state update.
s1 = zeros(3,1);
s1x = zeros(3,3);
s1los = zeros(3,1);
c = zeros(3,3);
Zi = zeros(4,3);
odiag = zeros(3,3);
s1b = zeros(3,1);
s1bx = zeros(3,3);
z = zeros(3,1);
State_Phi = zeros(12,12);
Q_tlt = zeros(3);
P_mat = zeros(12,12);
pm = zeros(12,12);
State_Vector = zeros(12,1);
sv = zeros(12,1);
resid_mat = zeros(3,3);
K_mat = zeros(12,3);
H_mat = zeros(3,12);
R_mat = zeros(3,3);
Y_mat = zeros(12,12);
y = zeros(12,1);
switch flag
%%%%%%%%%%%%%%%%%%
% Initialization %
%%%%%%%%%%%%%%%%%%
case 0
[sys,x0,str,ts] = mdlInitializeSizes(dt,tiltxi,tiltyi,tiltzi,gbxi,gbyi,gbzi,gsfxi,gsfyi,gsfzi,dx,dy,dz);
%%%%%%%%%%%%%%%%%%%%%%%%
% Others & Terminate %
%%%%%%%%%%%%%%%%%%%%%%%%
case {1,9}
sys = []; % do nothing
%%%%%%%%%%%%%%%
% Update %
%%%%%%%%%%%%%%%
case 2
sys = mdlUpdate(t,x,u,dt);
%%%%%%%%%%
% Output %
%%%%%%%%%%
case 3
sys = mdlOutputs(t,x,u);
%%%%%%%%%%%%%%%
% Update %
%%%%%%%%%%%%%%%
case 4
sys = mdlGetTimeofNextVarHit(t,x,u,dt);
otherwise
error(['unhandled flag = ',num2str(flag)]);
end
% end
%
%=============================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=============================================================================
%
function [sys,x0,str,ts] = mdlInitializeSizes(dt,tiltxi,tiltyi,tiltzi,gbxi,gbyi,gbzi,gsfxi,gsfyi,gsfzi,dx,dy,dz)
sizes = simsizes;
sizes.NumContStates = 0;
sizes.NumDiscStates =12*(12+1)/2+12;
sizes.NumOutputs =12;
sizes.NumInputs =13;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;
%
sys = simsizes(sizes);
str = [];
dqb1i = 0.5*tiltxi;
dqb2i = 0.5*tiltyi;
dqb3i = 0.5*tiltzi;
x0 = 0.0;
x0(1) = dqb1i^2;
x0(13) = dqb2i^2;
x0(24) = dqb3i^2;
x0(34) = gbxi^2;
x0(43) = gbyi^2;
x0(51) = gbzi^2;
x0(58) = gsfxi^2;
x0(64) = gsfyi^2;
x0(69) = gsfzi^2;
x0(73) = dx^2;
x0(76) = dy^2;
x0(78) = dz^2;
x0(90) = 0.0;
ts = [dt 0]; % sample time: [period, offset]
% end mdlInitializeSizes
%
%=============================================================================
% mdlUpdate
% Compute updates for discrete states.
%=============================================================================
%
function sys = mdlUpdate(t,x,u,dt)
%
NumberStates = 12;
s1 = [ u(11);
u(12);
u(13) ];
s1x = [ 0.0 -s1(3,1) s1(2,1);
s1(3,1) 0.0 -s1(1,1);
-s1(2,1) s1(1,1) 0.0 ];
q1 = u(1);
q2 = u(2);
q3 = u(3);
q0 = u(4);
ox = u(5);
oy = u(6);
oz = u(7);
c(1,1) = q0^2+q1^2-q2^2-q3^2;
c(1,2) = 2.0*(q1*q2-q0*q3);
c(1,3) = 2.0*(q1*q3+q0*q2);
c(2,1) = 2.0*(q1*q2+q0*q3);
c(2,2) = q0^2-q1^2+q2^2-q3^2;
c(2,3) = 2.0*(q2*q3-q0*q1);
c(3,1) = 2.0*(q1*q3-q0*q2);
c(3,2) = 2.0*(q2*q3+q0*q1);
c(3,3) = q0^2-q1^2-q2^2+q3^2;
Zi = [ -q0 -q3 q2;
q3 -q0 -q1;
-q2 q1 -q0;
q1 q2 q3 ];
odiag = [ ox 0.0 0.0;
0.0 oy 0.0;
0.0 0.0 oz ];
s1b = c'*s1;
s1bx = [ 0 -s1b(3,1) s1b(2,1);
s1b(3,1) 0 -s1b(1,1);
-s1b(2,1) s1b(1,1) 0 ];
obx = [ 0.0 -oz oy;
oz 0.0 -ox;
-oy ox 0.0 ];
k = 0;
for i=1:NumberStates
for j=i:NumberStates
k = k+1;
P_mat(i,j) = x(k);
P_mat(j,i) = P_mat(i,j);
end
State_Vector(i,1) = x(i+78);
end
%
% mapping to MATLAB states
% covariance matrix
% P_mat(1,1) = x(1); P_mat(1,2) = x(2); P_mat(1,3) = x(3); P_mat(1,4) = x(4); ...
% P_mat(2,2) = x(13); P_mat(2,3) = x(14); P_mat(2,4) = x(15); ...
% P_mat(3,3) = x(24); P_mat(3,4) = x(25); ...
% P_mat(4,4) = x(34); ...
% state vector
% State_Vector(1) = x(79); ...
State_Phi = [(eye(3,3)-obx*dt) -0.5*eye(3,3)*dt -0.5*odiag*dt zeros(3,3);
zeros(3,3) eye(3,3) zeros(3,3) zeros(3,3);
zeros(3,3) zeros(3,3) eye(3,3) zeros(3,3);
zeros(3,3) zeros(3,3) zeros(3,3) eye(3,3) ];
Q_tlt(1) = 0.25*(0.005/(57.296*60.0))^2;
Q_tlt(2) = 0.25*(0.005/(57.296*60.0))^2;
Q_tlt(3) = 0.25*(0.005/(57.296*60.0))^2;
% state vector propagation
sv = State_Phi*State_Vector;
State_Vector = sv;
% propagate error covariance matrix
pm = State_Phi*P_mat*State_Phi';
P_mat = pm;
for istate = 1:3
P_mat(istate,istate) = P_mat(istate,istate) + Q_tlt(istate)*dt;
end
% measurement updates
s1los(1,1) = u(8);
s1los(2,1) = u(9);
s1los(3,1) = u(10);
z = s1los - c'*s1;
H_mat = [ 2.0*s1bx zeros(3,3) zeros(3,3) -s1bx ];
R_var = (3.0e-5)^2;
R_mat(1,1) = R_var;
R_mat(2,2) = R_var;
R_mat(3,3) = R_var;
Y_mat = eye(NumberStates);
resid_mat = H_mat*P_mat*H_mat' + R_mat;
K_mat = P_mat*H_mat'*inv(resid_mat);
pm = (Y_mat - K_mat*H_mat)*P_mat;
P_mat = pm;
sv = State_Vector + K_mat*(z - H_mat*State_Vector);
State_Vector = sv;
k = 0;
for i=1:NumberStates
for j=i:NumberStates
k = k+1;
x(k) = P_mat(i,j);
end
x(i+78) = State_Vector(i,1);
end
sys = x;
% end mdlUpdate
%
%=============================================================================
% mdlOutputs
% Return the output vector for the S-function
%=============================================================================
%
function sys = mdlOutputs(t,x,u)
q1 = u(1);
q2 = u(2);
q3 = u(3);
q0 = u(4);
c(1,1) = q0^2+q1^2-q2^2-q3^2;
c(1,2) = 2.0*(q1*q2-q0*q3);
c(1,3) = 2.0*(q1*q3+q0*q2);
c(2,1) = 2.0*(q1*q2+q0*q3);
c(2,2) = q0^2-q1^2+q2^2-q3^2;
c(2,3) = 2.0*(q2*q3-q0*q1);
c(3,1) = 2.0*(q1*q3-q0*q2);
c(3,2) = 2.0*(q2*q3+q0*q1);
c(3,3) = q0^2-q1^2-q2^2+q3^2;
Zi = [ -q0 -q3 q2;
q3 -q0 -q1;
-q2 q1 -q0;
q1 q2 q3 ];
tiltx = 2.0*( c(1,1)*x(79) + c(1,2)*x(80) + c(1,3)*x(81) );
tilty = 2.0*( c(2,1)*x(79) + c(2,2)*x(80) + c(2,3)*x(81) );
tiltz = 2.0*( c(3,1)*x(79) + c(3,2)*x(80) + c(3,3)*x(81) );
y(1) = tiltx;
y(2) = tilty;
y(3) = tiltz;
y(4) = x(82);
y(5) = x(83);
y(6) = x(84);
y(7) = x(85);
y(8) = x(86);
y(9) = x(87);
y(10) = x(88);
y(11) = x(89);
y(12) = x(90);
sys = y;
% end mdlOutputs
%=============================================================================
% mdlGetTimeofNextVarHit
%=============================================================================
function sys = mdlGetTimeofNextVarHit(t,x,u,dt)
sys = t + dt;
% end mdlGetTimeofNextVarHit
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -