%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%?title:?Fault?Detection?based?On?Residual?In?Integrated?Navigation?System
%Author:?Liu?Jianxin
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
clear?all;clc;
Re?=?6378245;%地球長半徑
e?=?1/298.257;??%地球扁率
wie?=?7.292e-5;?%地球自轉角速度
McNumber?=?1000;
InsData?=?textread（‘INS_Data.txt‘）;%讀入數據
%真實航跡
RealAcc?=?InsData（:1:3）;%真實加速度
RealVel?=?InsData（:4:6）;%真實速度
RealPos?=?InsData（:7:9）;%真實經緯高
%慣導數據
Ins_Acc?=?InsData（:10:12）;%慣導加速度
Ins_Vel?=?InsData（:13:15）;%慣導速度
Ins_Pos?=?InsData（:16:18）;%慣導位置
Ins_Atd?=?InsData（:19:22）;%姿態四元素
Ins_Sfc?=?InsData（:23:25）;%地理系下的比力

%慣導數據的統計信息
Q_wg?=?（0.04/（57*3600））^2;??%陀螺馬氏過程
Q_wr?=?（0.01/（57*3600））^2;%陀螺白噪聲
Q_wa?=?（1e-3）^2;%加計馬氏過程
Q?=?diag（[Q_wg?Q_wg?Q_wg??Q_wr?Q_wr?Q_wr??Q_wa?Q_wa?Q_wa]）;%系統噪聲
Tg?=?300*ones（31）;?%陀螺的相關時間
Ta?=?1000*ones（31）;%加速度計的相關時間
for?k?=?1:McNumber
%GPS數據
Sam_Pos?=?RealPos（1:10:end:）;%采樣數據
n1?=?size（Sam_Pos1）;%數據量
Gps_Err（:1:2）?=?60*randn（n12）/Re;%GPS經緯度誤差
Gps_Err（:3）?=?60*randn（n11）;%GPS高度誤差
Gps_Pos?=?Sam_Pos+Gps_Err;%GPS位置
Gps_R?=?diag（std（Gps_Err）.^2）;%GPS測量噪聲方差
GpsSamTime?=?1;
%加入故障
for?i?=?50:150
????Gps_Pos（i1）?=?Gps_Pos（i1）+（i-50）*0.000001;
????Gps_Pos（i2）?=?Gps_Pos（i2）+（i-50）*0.000001;
end

%Cns數據
Sam_Pos2?=?RealPos（1:10:end:）;%采樣數據
n2?=?size（Sam_Pos21）;%數據量
Cns_Err（:1:2）?=?60*randn（n22）/Re;%Cns經緯度誤差
Cns_Err（:3）?=?60*randn（n21）;%Cns高度誤差
Cns_Pos?=?Sam_Pos2+Cns_Err;%Cns位置
Cns_R?=?diag（std（Cns_Err）.^2）;%Cns測量噪聲方差
CnsSamTime?=?1;
%加入故障
for?i?=?200:300
????Cns_Pos（i1）?=?Cns_Pos（i1）+（i-200）*0.000002;
????Cns_Pos（i2）?=?Cns_Pos（i2）+（i-200）*0.000002;
end
%%
%時間對準
InsAccGps?=?Ins_Acc（1:10:end:）;
InsVelGps?=?Ins_Vel（1:10:end:）;
InsPosGps?=?Ins_Pos（1:10:end:）;
InsAtdGps?=?Ins_Atd（1:10:end:）;
InsSfcGps?=?Ins_Sfc（1:10:end:）;
%將慣導數據與GPS數據對準
GpsMeasErr?=?InsPosGps-Gps_Pos;%慣導與GPS位置差——卡爾曼濾波的量測

n1?=?size（GpsMeasErr1）;%濾波拍數
Sta_Est_Gps?=?zeros（18n1）;%狀態估計
Cov_Est_Gps?=?zeros（1818n1）;%狀態協方差
Cov_Est_Gps（::1）?=?100*diag（[1/（36*57）?1/（36*57）?1/57?0.0001?0.0001?0.0001?1?1?1...
????0.1/（57*3600）?0.1/（57*3600）?0.1/（57*3600）?0.04/（57*3600）?0.04/（57*3600）?0.04/（57*3600）?1e-4?1e-4?1e-4].^2）;?%協方差初始化
label_Gps?=?zeros（n11）;
%東北天速度
ve?=?InsVelGps（:1）;%東向速度
vn?=?InsVelGps（:2）;%北向速度
vu?=?InsVelGps（:3）;%天向速度
L?=?InsPosGps（:1）;%慣導經度
h?=?InsPosGps（:3）;%慣導高度
%計算離散系統量測陣
H?=?zeros（318）;%離散系統量測陣
H（17）?=?1;
H（28）?=?1;
H（39）?=?1;
H_Gps?=?H;
F_Cns?=?zeros（1818n1）;%連續系統狀態轉移矩陣
%時間對準
InsAccCns?=?Ins_Acc（1:10:end:）;
InsVelCns?=?Ins_Vel（1:10:end:）;
InsPosCns?=?Ins_Pos（1:10:end:）;
InsAtdCns?=?Ins_Atd（1:10:end:）;
InsSfcCns?=?Ins_Sfc（1:10:end:）;
%將慣導數據與GPS數據對準
CnsMeasErr?=?InsPosCns-Cns_Pos;%慣導與GPS位置差——卡爾曼濾波的量測
n2?=?size（CnsMeasErr1）;
Sta_Est_Cns?=?zeros（18n2）;%狀態估計
Cov_Est_Cns?=?zeros（1818n2）;%狀態協方差
Cov_Est_Cns（::1）?=?100*diag（[1/（36*57）?1/（36*57）?1/57?0.0001?0.0001?0.0001?1

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件?????2222540??2013-09-22?08:13??基于模糊的故障診斷\INS_Data.txt
?????文件???????13730??2013-12-23?10:56??基于模糊的故障診斷\IntErrSmt.m
?????文件??????????40??2013-10-15?11:23??基于模糊的故障診斷\程序說明.txt