function [dx]=Quat(u)
% This function implements the quaternion integration as described
% in Addendix D of the book
% Aided Navigation: GPS and high rate sensors
% Jay A. Farrell, 2008, Mc Graw-Hill
%
% This software is distibuted without a written or implied warranty.
% The software is for educational purposes and is not intended for
% use in applications. Adaptation for applications is at the
% users/developers risk.
approach = 2;
w_bi_b= -u(5:7); % angular rate of body wrt inertial in body
if approach ==1,
b1 = u(1);
b_v(1:3,1) = u(2:4);
Omga =[-b_v'
b1*eye(3,3)-cross(b_v)]/2;
dx = Omga*w_bi_b;
else
b(1:4,1) = u(1:4);
Q = [0 -w_bi_b'
w_bi_b cross(w_bi_b)];
dx = Q*b/2;
end
function [y]=cross(x)
y = [ 0 -x(3) x(2)
x(3) 0 -x(1)
-x(2) x(1) 0];