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

%慣導(dǎo)數(shù)據(jù)的統(tǒng)計(jì)信息
Q_wg?=?（0.04/（57*3600））^2;??%陀螺馬氏過(guò)程
Q_wr?=?（0.01/（57*3600））^2;%陀螺白噪聲
Q_wa?=?（1e-3）^2;%加計(jì)馬氏過(guò)程
Q?=?diag（[Q_wg?Q_wg?Q_wg??Q_wr?Q_wr?Q_wr??Q_wa?Q_wa?Q_wa]）;%系統(tǒng)噪聲
Tg?=?300*ones（31）;?%陀螺的相關(guān)時(shí)間
Ta?=?1000*ones（31）;%加速度計(jì)的相關(guān)時(shí)間
for?k?=?1:McNumber
%GPS數(shù)據(jù)
Sam_Pos?=?RealPos（1:10:end:）;%采樣數(shù)據(jù)
n1?=?size（Sam_Pos1）;%數(shù)據(jù)量
Gps_Err（:1:2）?=?60*randn（n12）/Re;%GPS經(jīng)緯度誤差
Gps_Err（:3）?=?60*randn（n11）;%GPS高度誤差
Gps_Pos?=?Sam_Pos+Gps_Err;%GPS位置
Gps_R?=?diag（std（Gps_Err）.^2）;%GPS測(cè)量噪聲方差
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數(shù)據(jù)
Sam_Pos2?=?RealPos（1:10:end:）;%采樣數(shù)據(jù)
n2?=?size（Sam_Pos21）;%數(shù)據(jù)量
Cns_Err（:1:2）?=?60*randn（n22）/Re;%Cns經(jīng)緯度誤差
Cns_Err（:3）?=?60*randn（n21）;%Cns高度誤差
Cns_Pos?=?Sam_Pos2+Cns_Err;%Cns位置
Cns_R?=?diag（std（Cns_Err）.^2）;%Cns測(cè)量噪聲方差
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
%%
%時(shí)間對(duì)準(zhǔn)
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:）;
%將慣導(dǎo)數(shù)據(jù)與GPS數(shù)據(jù)對(duì)準(zhǔn)
GpsMeasErr?=?InsPosGps-Gps_Pos;%慣導(dǎo)與GPS位置差——卡爾曼濾波的量測(cè)

n1?=?size（GpsMeasErr1）;%濾波拍數(shù)
Sta_Est_Gps?=?zeros（18n1）;%狀態(tài)估計(jì)
Cov_Est_Gps?=?zeros（1818n1）;%狀態(tài)協(xié)方差
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）;?%協(xié)方差初始化
label_Gps?=?zeros（n11）;
%東北天速度
ve?=?InsVelGps（:1）;%東向速度
vn?=?InsVelGps（:2）;%北向速度
vu?=?InsVelGps（:3）;%天向速度
L?=?InsPosGps（:1）;%慣導(dǎo)經(jīng)度
h?=?InsPosGps（:3）;%慣導(dǎo)高度
%計(jì)算離散系統(tǒng)量測(cè)陣
H?=?zeros（318）;%離散系統(tǒng)量測(cè)陣
H（17）?=?1;
H（28）?=?1;
H（39）?=?1;
H_Gps?=?H;
F_Cns?=?zeros（1818n1）;%連續(xù)系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣
%時(shí)間對(duì)準(zhǔn)
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:）;
%將慣導(dǎo)數(shù)據(jù)與GPS數(shù)據(jù)對(duì)準(zhǔn)
CnsMeasErr?=?InsPosCns-Cns_Pos;%慣導(dǎo)與GPS位置差——卡爾曼濾波的量測(cè)
n2?=?size（CnsMeasErr1）;
Sta_Est_Cns?=?zeros（18n2）;%狀態(tài)估計(jì)
Cov_Est_Cns?=?zeros（1818n2）;%狀態(tài)協(xié)方差
Cov_Est_Cns（::1）?=?100*diag（[1/（36*57）?1/（36*57）?1/57?0.0001?0.0001?0.0001?1

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