Download fulltext - DiVA Portal

B.1. MATLAB
79
O3 O3 O3];
%B = [O3; I3];
D = [O3 O3; I3 O3; O3 I3];
C = [I3 O3 O3];
bias = zeros(3,1);
% Calculating gravity
lambda = 63.36;
gamma_a = 9.78049;
f = 1/298.257223563;
m = 0.00344978650684;
f_2 = -f+5/2*m+1/2*f^2-26/7*f*m+15/4*m^2;
f_4 = -1/2*f^2+5/2*f*m;
g_const = gamma_a*(1+(f_2+f_4)*sind(lambda)^2-1/4*f_4*sind(2*lambda)^2);
% This is the gravity calculated using the gravity model
% Flag to denote that imu measurement passes all tests
imu_isValid = 0;
prev_time = Inf;
fclose(instrfind)
delete(instrfind)
imuopen;
% Defining gain constants for accelerometer and gyro.
g_acc = 9.81*7000/32768000;
g_gyro = 360/65536;
g_rate = 8500/32768000;
%cmd = hex2dec(['10'; '00'; '31'])';
% Telling the IMU to write its data
%fwrite(s_imu, cmd ,'uint8');
%out = fread(s_imu, 7, 'uint8');
while(1)
imureader;
end
B.1.11
updateR.m
function [Rb2t] = updateR(phi, theta, psi)
Rb2t = [cosd(psi)*cosd(theta)
-sind(psi)*cosd(phi)+cosd(psi)*sind(theta)*sind(phi)
sind(psi)*sind(phi)+cosd(psi)*sind(theta)*cosd(phi);