INS/GPS matlab仿真程序

matlab仿真程序，用于计算INS/GPS模拟和仿真。

%
%   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