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

代码片段和文件信息
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
相关资源
- 卡尔曼滤波与组合导航原理_第三版
- 卡尔曼滤波与组合导航原理
- 交互多模IMM 卡尔曼滤波
- 卡尔曼滤波、自适应卡尔曼、抗差卡
- 卡尔曼滤波初学详解,包你懂!
- 滤波-卡尔曼滤波-互补滤波
- 捷联惯导算法与组合导航原理讲义-严
- 双向不敏卡尔曼滤波的无源定位算法
- 容积卡尔曼滤波(CKF)和嵌入式容积
- GPS&INS;组合导航
- 卡尔曼滤波与组合导航
- 基于Verilog HDL的卡尔曼滤波器的设计
- 捷联惯导系统原理_陈哲
- 卡尔曼滤波理论与实践英文原版+代码
- 捷联惯导算法与组合导航原理 严
- 严恭敏老师捷联惯导算法与组合导航
- 卡尔曼滤波 宋文尧
- GNSS与惯性及多传感器组合导航系统原
- mpu6050+地磁传感器通过卡尔曼滤波得出
- K5环境+STM32+MPU6050+卡尔曼滤波源码
- 一种改进扩展卡尔曼滤波新方法
- 基于扩展卡尔曼滤波EKF的机器人SLAM问
- 卡尔曼滤波器及其应用基础.pdf
- Fundamentals of Kalman Filtering A Practical A
- 平衡车卡尔曼滤波
- 卡尔曼滤波理论及其在导航系统中的
- kalman滤波理论及其在导航系统中的应
- 捷联惯导算法与组合导航原理讲义2
- Fundamentals of Inertial Navigation
- 捷联惯导算法与组合导航原理讲义(
评论
共有 条评论