初始对准程序

捷联惯导系统静基座初始对准 MATLAB

%%%%%%%%%%%采用东北天坐标系%%%%%%%%%%%%%%
%%%%%%%%%%%%初始对准仿真%%%%%%%%%%%%%%%%%
%%%%%%%%%%%加速度计的零偏和陀螺漂移都考虑的情况%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%给定常数%%%%%%%%%%%%%%%%%%%

%初始姿态角（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