%--------------------------------------------------------------------------
% 函数原型：[Wbi,Qbi,Ri,Vi]=Dynamic(II,Wbi,Qbi,Ri,Vi,Tall,Ap,step)
% 功能；
% 实现轨道动力学方程和姿态动力学方程
% 输入：
% II[3][3] 为卫星的主轴惯量矩阵 本体坐标系 单位 kg*m^2
% Wbi[3] 卫星本体坐标系相对于惯性系的角速度 本体坐标系 单位 rad/s
% Qbi[4] 卫星本体坐标系相对于惯性系的姿态四元素 标量放在第四位
% Ri[3] 卫星在惯性系中的位置矢量 惯性坐标系 单位 m
% Vi[3] 卫星在惯性系中的速度矢量 惯性坐标系 单位 m/s
% Tall[3] 卫星所受外力矩 本体坐标系 单位 Nm
% Ap[3] 由摄动力和推进器力造成的加速度 惯性坐标系 单位 m/s^2
% step 仿真时间间隔 单位 s
% 输出：
% 更新之后的Wbi,Qbi,Ri,Vi
% 参考文献：
% 章仁为,《卫星轨道姿态动力学与控制》 P148
%《航天器姿态动力学与控制（讲义）》 P45
%--------------------------------------------------------------------------
% 作者：赖镇洲
% 时间：2012.3.26
% 测试者：赖镇洲
% 测试时间：2012.3.26
% 测试结果：无语法错误，结果合理
%--------------------------------------------------------------------------
function [rWbi,rQ,rSensor]=FaultDiagnosis(II,sWbi,sQbi,Tall,step)
% function [rWbi,eWbi]=FaultDiagnosis(II,Wbi,eWbi,Tall,step)
global eWbi;
global eQbi;
global DataWbi;
%------------------------- 执行器故障诊断 ----------------------------------
L = 0.05;
% 故障诊断观测器
dW = [(Tall(1)+(II(2,2)-II(3,3))*sWbi(2)*sWbi(3))/II(1,1);
(Tall(2)+(II(3,3)-II(1,1))*sWbi(3)*sWbi(1))/II(2,2);
(Tall(3)+(II(1,1)-II(2,2))*sWbi(1)*sWbi(2))/II(3,3)] + L*(sWbi-eWbi);
eWbi=eWbi+dW*step;
rWbi = sWbi-eWbi;
% 姿态运动学观测器
dQ = [0.5*( sWbi(3)*sQbi(2) -sWbi(2)*sQbi(3) +sWbi(1)*sQbi(4));
0.5*(-sWbi(3)*sQbi(1) +sWbi(1)*sQbi(3) +sWbi(2)*sQbi(4));
0.5*( sWbi(2)*sQbi(1) -sWbi(1)*sQbi(2) +sWbi(3)*sQbi(4));
0.5*(-sWbi(1)*sQbi(1) -sWbi(2)*sQbi(2) -sWbi(3)*sQbi(3))]+L*(sQbi-eQbi);
eQbi=eQbi+dQ*step;
LQ=sqrt(eQbi(1)*eQbi(1)+eQbi(2)*eQbi(2)+eQbi(3)*eQbi(3)+eQbi(4)*eQbi(4));
eQbi=eQbi/LQ;
rQ = sQbi-eQbi;
%------------------------- 传感器故障诊断 ----------------------------------
N = length(DataWbi);
for k = 1:(N-1)
DataWbi(k,:) = DataWbi(k+1,:);
end
DataWbi(N,:) = sWbi;
dData = diff(DataWbi);
rSensor = [var(dData(:,1)); var(dData(:,2)); var(dData(:,3))];
end
%-------------------测试代码------------------------------------------------
% II=[24.09 0 0;
% 0 32.1 0;
% 0 0 31.47]; % 卫星主轴惯量
% Ri=[-2561875.2;-3511978.8;5786546.4]; % 卫星位置初值
% Vi=[-4722.7;-3872.4;-4277.9]; % 卫星速度初值
% Tall=[0;0;0]; % 力矩初值
% Wbi=[0.1;0.1;0.1]; % 角速度初值
% Qbi=[0.3617;-0.8382;0.1281;0.3876]; % 四元数初值
% step=0.02; % 仿真步长 设置为0.02 s
% Ap=[0;0;0];
% [Wbi,Qbi,Ri,Vi]=Dynamic(II,Wbi,Qbi,Ri,Vi,Tall,Ap,step)
