zuhedaohang 捷联惯导和GPS组合导航程序

捷联惯导和GPS组合导航程序，包含初始对准和滤波算法

function [XP] = ahrsupdate14（measAttitude xyz uvw Q delrate g P）
% This will update the KF gains with an AHRS-like measurement
%
% [XP] = ahrsupdate（measAttitude xyz uvw Q g P）
%
%   Inputs:
%   measAttitude = [phi; theta; psi]
%   xyz = [X; Y; Z] （est）
%   uvw = [u; v; w] （est）
%   Q = [q0; q1; q2; q3] （est）
%	 delrate = [delp; delq; delr] （est）
%   g = g （est）
%   P = coverience matrix
%
% X = [x; y; z; u; v; w; q0; q1; q2; q3; delp; delq; delr; g];
% P = updated coverience matrix

global CompSD;

x = [xyz; uvw; Q; delrate; g];

R = diag（[CompSD^2 CompSD^2 CompSD^2]）;

% converting heading into quaterinon
eul = quat2euler（Q‘）;


C = zeros（314）;
C（1 [7:10]） = dphidq（Q‘）;
C（2 [7:10]） = dthetadq（Q‘）;
C（3 [7:10]） = dpsidq（Q‘）;

% get the E matrix
E = C*P*C‘ + R;

% compute t

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       1393  2002-10-05 10:05  gpsins\ahrsupdate14.m

     文件        345  2002-10-05 09:29  gpsins\alphabeta.m

     文件        839  2002-10-05 10:04  gpsins\compassUpdate.m

     文件        841  2002-10-05 09:32  gpsins\compassUpdate11.m

     文件        915  2002-10-05 09:31  gpsins\compassUpdate14.m

     文件       1709  2002-10-05 09:29  gpsins\contents.m

     文件        260  2002-10-05 09:25  gpsins\dms2deg.m

     文件        528  2002-10-05 09:29  gpsins\dphidq.m

     文件        530  2002-10-05 09:29  gpsins\dpsidq.m

     文件        396  2002-10-05 09:29  gpsins\dthetadq.m

     文件        596  2002-10-05 09:30  gpsins\ecef2llh.m

     文件        500  2002-10-05 09:25  gpsins\ecef2tangent.m

     文件        636  2002-10-05 09:29  gpsins\euler2quat.m

     文件        655  2009-10-15 21:05  gpsins\eulerDCM.asv

     文件        518  2002-10-05 09:29  gpsins\eulerDCM.m

     文件        243  2002-10-05 09:29  gpsins\eulerWx.m

     文件        386  2002-10-05 09:29  gpsins\eulStrap.m

     文件        987  2002-10-05 09:32  gpsins\gen11a.m

     文件       1966  2002-10-05 09:31  gpsins\gen14a.m

     文件        985  2002-10-05 10:04  gpsins\gena.m

     文件       1056  2009-10-16 17:11  gpsins\gpsUpdate.asv

     文件       1049  2002-10-05 10:04  gpsins\gpsUpdate.m

     文件       1051  2002-10-05 09:32  gpsins\gpsUpdate11.m

     文件       1125  2002-10-05 09:31  gpsins\gpsUpdate14.m

     文件       1171  2002-10-05 09:29  gpsins\grav.m

     文件       6176  2007-01-26 15:51  gpsins\kf.asv

     文件       6208  2010-03-24 21:59  gpsins\kf.m

     文件       6207  2002-10-05 09:32  gpsins\kf11.m

     文件       7668  2010-03-26 09:31  gpsins\kf14.asv

     文件       7668  2010-03-26 09:31  gpsins\kf14.m

............此处省略84个文件信息