%
%???aided_ins_15_state.m
%
%???Inertial?Navigation?System?with?15-state?Kalman?filter?using
%???position-aiding
%
%???This?program?performs?the?integration?Kalman?filtering?and
%???thus?takes?input?from?programs?which?have?already?generated
%???the?INS?data.??See?the?LOAD?command?below.
%
%???The?state?vector?is:
%????x1?=?delta?x?position
%????x2?=?delta?y?position
%????x3?=?delta?z?position
%????x4?=?delta?x?velocity
%????x5?=?delta?y?velocity
%????x6?=?delta?z?velocity
%????x7?=?psi-angle?x?component
%????x8?=?psi-angle?y?component
%????x9?=?psi-angle?z?component
%???x10?=?body-x?accel?bias
%???x11?=?body-y?accel?bias
%???x12?=?body-z?accel?bias
%???x13?=?body-x?gyro?bias
%???x14?=?body-y?gyro?bias
%???x15?=?body-z?gyro?bias
%
%????Note:??the?position?and?velocity?errors?are?expressed?in?the
%???????????so-called?local-level?frame?or?simply?L?（or?l）?frame.
%???????????In?the?L-frame:??x=east;?y=north;?z=up
%
%??August?2005
%??Copyright?（c）?2005?by?GPSoft?LLC
%??All?Rights?Reserved.

clear?all
close?all

fly_over_rms?=?3;??%?RMS?error?（in?meters）?of?the?external?2-D?position
%??????????????????%?fix?（in?each?dimension）;?also?known?as?a?fly-over?fix

load?dyn_flt_ins_dat????%?output?from?the?program?DYN_FLIGHT_INS.M

randn（‘state‘0）

C?=?[0?1?0;?1?0?0;?0?0?-1];????%?conversion?between?NED?and?ENU

H?=?zeros（215）;??H（11）?=?1;??H（22）?=?1;
%?????????????????????????????????%?The?measurement?vector?z?consists?of
%?????????????????????????????????%?an?x?and?y?position?difference?only.
%?????????????????????????????????%?Since?we?are?working?in?the?L?frame
%?????????????????????????????????%?this?corresponds?to?east?and?north
%?????????????????????????????????%?position?difference?（since?alpha=0）.

Peast_pos?=?fly_over_rms^2;??Pnorth_pos?=?fly_over_rms^2;??Pup_pos?=?0;
Peast_vel?=?2^2;??Pnorth_vel?=?2^2;??Pup_vel?=?0;
Ppsi_x?=?0.0001^2;??Ppsi_y?=?0.0001^2;??Ppsi_z?=?0.0001^2;
Pacc_x?=?（100*9.81e-6）^2;?Pacc_y?=?（100*9.81e-6）^2;?Pacc_z?=?（100*9.81e-6）^2;
Pgyr_x?=?（0.05）^2;?Pgyr_y?=?（0.05）^2;?Pgyr_z?=?（0.05）^2;

P_pre?=?zeros（1515）;
P_pre（11）=Peast_pos;?P_pre（22）=Pnorth_pos;?P_pre（33）=Pup_pos;
P_pre（44）=Peast_vel;?P_pre（55）=Pnorth_vel;?P_pre（66）=Pup_vel;
P_pre（77）=Ppsi_x;?P_pre（88）=Ppsi_y;?P_pre（99）=Ppsi_z;
P_pre（1010）=Pacc_x;?P_pre（1111）=Pacc_y;?P_pre（1212）=Pacc_z;
P_pre（1313）=Pgyr_x;?P_pre（1414）=Pgyr_y;?P_pre（1515）=Pgyr_z;

P_pre_KF?=?P_pre;???%?initial?prediction?error?covariance?matrix

R?=?[fly_over_rms^2?0;?0?fly_over_rms^2];??%?measurement?error?covariance?matrix

sigma_acc?=?0.3*9.81e-6;
sigma_gyr?=?1e-9;
G?=?zeros（1515）;?
G（1010）=sigma_acc;?G（1111）=sigma_acc;?G（1212）=sigma_acc;?
G（1313）=sigma_gyr;?G（1414）=sigma_gyr;?G（1515）=sigma_gyr;?
W?=?zeros（1515）;?
W（1010）=1;?W（1111）=1;?W（1212）=1;
W（1313）=1;?W（1414）=1;?W（1515）=1;

tau_accel?=?100000;
tau_gyro?=?100000;
update?=?0;??count?=?0;

X_pre?=?zeros（151）;

est_lat_KF（1）?=?lat_pr

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件??????10607??2005-09-29?13:00??aided_ins_15_state.m

-----------?---------??----------?-----??----

????????????????10825????????????????????2