資源簡介
驗證有效的GPS-INS 組合導航代碼,可以和我上傳的GPS/BD receiver, 一起學習

代碼片段和文件信息
%%%%%%%%%%%%%%%%%捷聯慣導GPS組合導航Kalman濾波器%%%%%%%%%%%%%%%%
clc;
clear;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Wie=7.292115e-5;?????????????????%地球自轉角速
e=1/298.257;?????????????????????%地球扁律
Re=6378137;??????????????????????%地球半徑
STEP=0.1;????????????????????????%慣導解算周期
G0=9.7803;???????????????????????%重力加速度初值
arc=pi/180;??????????????????????%角度轉換
T=0.3;???????????????????????????%濾波周期,單位秒
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
load?y_Ishuju;
load?y_Gshuju;
S_dat_I=y_I;
S_dat_G=y_G;
%%%%%%%%%%%%%%%%%%%%數據加載%%%%%%%%%%%%%%%%%%%%%%%%%%%
I=eye(13);
A=zeros(13);
X00=zeros(131);
X00(11)=(10/60)*arc;
X00(21)=(10/60)*arc;
X00(31)=(10/60)*arc;
X00(41)=10;
X00(51)=10;
X00(61)=(4/60)*arc;
X00(71)=(4/60)*arc;
X00(81)=(0.5/3600)*arc;
X00(91)=(0.5/3600)*arc;
X00(101)=(0.5/3600)*arc;
X00(111)=(1e-4)*9.81;
X00(121)=(1e-4)*9.81;
X00(131)=(1e-4)*9.81;
P00=zeros(13);?????????????????????????????????????????????%Klman濾波中估計誤差方差陣初值
P00(11)=((10/60)*arc)*((10/60)*arc);
P00(22)=((10/60)*arc)*((10/60)*arc);
P00(33)=((10/60)*arc)*((10/60)*arc);
P00(44)=10*10;
P00(55)=10*10;
P00(66)=((4/60)*arc)*((4/60)*arc);
P00(77)=((4/60)*arc)*((4/60)*arc);
P00(88)=((0.5/3600)*arc)*((0.5/3600)*arc);
P00(99)=((0.5/3600)*arc)*((0.5/3600)*arc);
P00(1010)=((0.5/3600)*arc)*((0.5/3600)*arc);
P00(1111)=((1e-4)*9.81)*((1e-4)*9.81);
P00(1212)=((1e-4)*9.81)*((1e-4)*9.81);
P00(1313)=((1e-4)*9.81)*((1e-4)*9.81);
Q0=zeros(13);??????????????????????????????????????????????%Klman濾波中過程噪聲方差陣初值
Q0(11)=((1/3)*((0.5/3600)*arc))*((1/3)*((0.5/3600)*arc));
Q0(22)=((1/3)*((0.5/3600)*arc))*((1/3)*((0.5/3600)*arc));
Q0(33)=((1/3)*((0.5/3600)*arc))*((1/3)*((0.5/3600)*arc));
Q0(44)=((1/3)*(1e-4)*9.81)*((1/3)*(1e-4)*9.81);
Q0(55)=((1/3)*(1e-4)*9.81)*((1/3)*(1e-4)*9.81);
R1=zeros(4);???????????????????????????????????????????????%Klman濾波中觀測噪聲方差陣初值
R1(11)=(0.3)^2;??????????????????????????????????????????
R1(22)=(0.3)^2;
R1(33)=(5/Re)^2;
R1(44)=(5/Re)^2;
H1=zeros(413);????????????????????????????????????????????%Klman濾波中觀測陣初值
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%濾波開始%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
mmm=round(T/STEP);
for?j=0:1:length(S_dat_I)/mmm-2
????
????Sum=0;
????
????for?DRDER=(j*mmm+1):1:(j+1)*mmm
????????
????????vx_G=S_dat_G(DRDER1);
????????vy_G=S_dat_G(DRDER2);
????????vz_G=S_dat_G(DRDER3);
????????
????????jd_G=S_dat_G(DRDER4);
????????wd_G=S_dat_G(DRDER5);
????????
????????Vx_I=S_dat_I(DRDER1);
????????Vy_I=S_dat_I(DRDER2);
????????Vz_I=S_dat_I(DRDER3);??????
????????
????????Q=S_dat_I(DRDER4);
????????R=S_dat_I(DRDER5);
????????W=S_dat_I(DRDER6);
????
????????JD=S_dat_I(DRDER7);
????????WD=S_dat_I(DRDER8);
????????H=S_dat_I(DRDER9);
????
????????fx_n=S_dat_I(DRDER10);
????????fy_n=S_dat_I(DRDER11);
????????fz_n=S_dat_I(DRDER12);
????????
????????%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
????
????????q000=cos(Q/2)*cos(R/2)*cos(W/2)-sin(Q/2)
?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2019-02-22?11:19??GPS-INS\
?????文件??????376832??2018-09-17?10:07??GPS-INS\INSGPS組合導航系統0510.ppt
?????文件????????7468??2008-04-27?16:47??GPS-INS\INSandGPS.m
?????文件??????737280??2008-04-27?16:18??GPS-INS\捷聯慣導0509.ppt
?????文件??????350208??2008-05-04?08:59??GPS-INS\組合導航若干基本問題.ppt
- 上一篇:echarts圖表接入天地圖高德地圖geo解決方案
- 下一篇:模型預測控制的S函數
評論
共有 條評論