组合导航仿真程序

利用卡尔曼滤波完成组合导航和分布式信息融合和故障检测

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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