%%%%%%%%%%%采用東北天坐標系%%%%%%%%%%%%%%
%%%%%%%%%%%%初始對準仿真%%%%%%%%%%%%%%%%%
%%%%%%%%%%%加速度計的零偏和陀螺漂移都考慮的情況%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%給定常數%%%%%%%%%%%%%%%%%%%

%初始姿態角（rad）
yaw_origin?=?0;
roll_origin?=?0;
pitch_origin?=?0;

N?=?5000;%仿真時間
deg?=?pi/180;%一度對應的弧度
latitude_origin?=?45*deg;%初始緯度（rad）
g?=?9.84;%地球重力加速度（m/s2）
Wie?=?7.2916e-5;%地球自轉角速度（rad/s）
%艦船初始速度（m/s）
VE_origin?=?0;
VN_origin?=?0;

%初始失準角
PE_origin?=?deg;
PN_origin?=?deg;
PU_origin?=?deg;

%慣性器件的隨機和常值漂移
Gyro_cons?=?0.02*deg/3600;
Gyro_rand?=?0.01*deg/3600;

Acce_cons?=?100e-6*g;
Acce_rand?=?50e-6*g;

%%%%%%%%設置Kalman濾波器的初始值%%%%%%%%
X0r?=?[0.10.1degdegdeg100e-6*g100e-6*g0.02*deg/36000.02*deg/36000.02*deg/3600]‘;%狀態初值，真實值
X0e?=?[0000000000]‘;%狀態估計初值
Q0?=?diag（[（50e-6*g）^2?（50e-6*g）^2?（0.01*deg/3600）^2?（0.01*deg/3600）^2?（0.01*deg/3600）^2?0?0?0?0?0]）;
P0?=?diag（[（0.1）^2?（0.1）^2?（deg）^2?（deg）^2?（deg）^2?（100e-6*g）^2（100e-6*g）^2（0.02*deg/3600）^2（0.02*deg/3600）^2（0.02*deg/3600）^2]）;
R?=?diag（[（0.1）^2?（0.1）^2]）;

%%%%%%%%%%%%%%%%根據慣導誤差方程列些狀態方程%%%%%%%%%%%%%%%%%%
Ou?=?Wie*sin（latitude_origin）;
On?=?Wie*cos（latitude_origin）;
F?=?[0?2*Ou?0?-g?0;
????-2*Ou?0?g?0?0;
????0?0?0?Ou?-On;
????0?0?-Ou?0?0;
????0?0?On?0?0];
Ci?=?eye（5）;%姿態矩陣寫成單位矩陣表示載體坐標系和導航坐標系重合
Ci?=[0.9994?0.0177?0?0?0;
????-0.0171?0.9994?0?0?0;
????0?0?0.9994?0.0177?-0.0171;
????0?0?-0.0171?0.9994?0.0177;
????0?0?0.0177?-0.0171?0.9994];
A?=?[F?Ci;
?????zeros（55）?zeros（55）];
B?=?eye（10）;
H?=?[eye（2）?zeros（28）];
D?=?zeros（210）;
%%%%%%%%%%%%%%%%%%對連續模型進行離散化的處理%%%%%%%%%%%%%%%%%
G?=?ss（ABHD）;
T?=?1;
GD?=?c2d（GT）;
PHI?=?GD.a;%%%%%離散化后的狀態轉移矩陣也可以寫成expm（A*T）
Xout?=?zeros（10N）;

%%%%%%%%%%%%%%%%%%%%進入濾波器的計算%%%%%%%%%%%%%%%%%%%%%%%%%
Xrk_1?=?X0r;
Xk_1?=?X0e;
Pk_1?=?P0;
for?i?=?1:N
??Z?=?H*Xrk_1?+?（mvnrnd（[0;0]R1））‘;??
??Pkk?=?PHI*Pk_1*PHI‘?+?Q0;%一步方差預測
??K?=?Pkk*H‘*inv（H*Pkk*H‘?+?R）;%濾波增益
??Pk?=?（eye（10）-K*H）*Pkk;????%估計均方誤差
??Xk?=?PHI*Xk_1;??%一步預測
??Xk?=?Xk?+?K*（Z?-?H*Xk）;
??%Xk?=?Xk;
??Xout（:i）?=?Xk;
??Xk_1?=?Xk;
??Pk_1?=?Pk;
??%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
??%%%%%%%%%%%%%%%估計出失準角應該更新姿態矩陣%%%%%%%%%%%%%%%%
??Ci（33）?=?Ci（33）?-?Xout（5i）*Ci（43）?+?Xout（4i）*Ci（53）;
??Ci（34）?=?Ci（34）?-?Xout（5i）*Ci（44）?+?Xout（4i）*Ci（54）;
??Ci（35）?=?Ci（35）?-?Xout（5i）*Ci（45）?+?Xout（4i）*Ci（55）;
??
??Ci（43）?=?Xout（5i）*Ci（33）?+?Ci（43）?-?Xout（3i）*Ci（53）;
??Ci（44）?=?Xout（5i）*Ci（34）?+?Ci（44）?-?Xout（3i）*Ci（54）;
??Ci（45）?=?Xout（5i）*Ci（35）?+?Ci（45）?-?Xout（3i）*Ci（55）;
??
??Ci（53）?=?-Xout（4i）*Ci（33）?+?Xout（3i）*Ci（43）?+?Ci（53）;
??Ci（54）?=?-Xout（4i）*Ci（34）?+?Xout（3i）*Ci（44）?+?Ci（54）;
??Ci（55）?=?-Xout（4i）*Ci（35）?+?Xout（3i）*Ci（45）?+?Ci（55）;
??%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
???Ci?=?[Ci（33）?Ci（34）?0?0?0;
????????Ci（43）?Ci（44）?0?0?0;
????????0?0?Ci（33）?Ci（34）?Ci（35）;
????????0?0?Ci（43）?Ci（44）?Ci（45）;
????????0?0?Ci（53）?Ci（54）?Ci（55）];
??A?=?[F

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

?????文件???????3629??2012-03-03?15:21??qk_AG.m

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

?????????????????3629????????????????????1