diff --git a/matlab/GNSSaidedINS_data.mat b/matlab/GNSSaidedINS_data.mat
new file mode 100644
index 0000000000000000000000000000000000000000..60b9f6e1c6b916a1fe14c166240399021974081c
Binary files /dev/null and b/matlab/GNSSaidedINS_data.mat differ
diff --git a/matlab/GPSaidedINS.m b/matlab/GPSaidedINS.m
new file mode 100644
index 0000000000000000000000000000000000000000..6d7a3474e526f41d4cac46099ac6d46cc26cdecd
--- /dev/null
+++ b/matlab/GPSaidedINS.m
@@ -0,0 +1,286 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+% function call: out_data=GPSaidedINS(in_data,settings)
+%
+% This function integrates GNSS and IMU data. The data fusion
+% is based upon a loosely-coupled feedback GNSS-aided INS approach.
+%
+% Input struct:
+% in_data.GNSS.pos_ned GNSS-receiver position estimates in NED
+% coordinates [m]
+% in_data.GNSS.t Time of GNSS measurements [s]
+% (in_data.GNSS.HDOP GNSS Horizontal Dilution of Precision [-])
+% (in_data.GNSS.VDOP GNSS Vertical Dilution of Precision [-])
+% in_data.IMU.acc Accelerometer measurements [m/s^2]
+% in_data.IMU.gyro Gyroscope measurements [rad/s]
+% in_data.IMU.t Time of IMU measurements [s]
+%
+% Output struct:
+% out_data.x_h Estimated navigation state vector [position; velocity; attitude]
+% out_data.delta_u_h Estimated IMU biases [accelerometers; gyroscopes]
+% out_data.diag_P Diagonal elements of the Kalman filter state
+% covariance matrix.
+%
+%
+% Edit: Isaac Skog (skog@kth.se), 2016-09-06
+% Revised: Bo Bernhardsson, 2018-01-01
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function out_data=GPSaidedINS(in_data,settings)
+
+
+% Copy data to variables with shorter name
+u=[in_data.IMU.acc;in_data.IMU.gyro];
+t=in_data.IMU.t;
+
+%% Initialization
+% Initialize the navigation state
+x_h=init_navigation_state(u,settings);
+
+% Initialize the sensor bias estimate
+delta_u_h=zeros(6,1);
+
+% Initialize the Kalman filter
+[P,Q1,Q2,~,~]=init_filter(settings);
+
+
+% Allocate memory for the output data
+N=size(u,2);
+out_data.x_h=zeros(10,N);
+out_data.x_h(:,1)=x_h;
+out_data.diag_P=zeros(15,N);
+out_data.diag_P(:,1)=diag(P);
+out_data.delta_u_h=zeros(6,N);
+
+%% Information fusion
+ctr_gnss_data=1;
+GPS_position=true;
+
+%% Initialize GPS sensor
+sensor.state='warm_start_available';
+sensor.howLong=0;
+sensor.ephExp=1800.0;
+sensor.fp=0;
+sensor.eph=5;
+turn=true;
+power=400;
+energy=0;
+
+pre_state='startup';
+a=0;
+
+for k=2:N
+
+ % Sampling period
+ Ts=t(k)-t(k-1);
+
+ %%%%%%%%%%%%%%%%%
+ %% GPS sensor %%
+ %%%%%%%%%%%%%%%%%
+ %number of visible satellites
+ sv=5;
+ %control action
+ a=[a,sum(sqrt(out_data.diag_P(1:3,k-1)))];
+ if sum(sqrt(out_data.diag_P(1:3,k-1)))<1.0
+ turn=false;
+ else
+ turn=true;
+ end
+ %update sensor state
+ pre_state=sensor.state;
+ sensor=gps(t(k),sensor,turn,sv);
+ %determine if GPS position is available
+ GPS_position=strcmp(sensor.state,'position_available');
+ %compute power consumption
+ if ~(strcmp(sensor.state,'no_info')||strcmp(sensor.state,'warm_start_available'))
+ energy=energy+power*Ts;
+ end
+
+% %print new state and current time if it changes
+% if ~strcmp(pre_state, sensor.state)
+% sensor.state
+% t(k)
+% end
+
+ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+ % Calibrate the sensor measurements using current sensor bias estimate.
+ u_h=u(:,k)+delta_u_h;
+
+ % Update the INS navigation state
+ x_h=Nav_eq(x_h,u_h,Ts);
+
+ % Get state space model matrices
+ [F,G]=state_space_model(x_h,u_h,Ts);
+
+ % Time update of the Kalman filter state covariance.
+ P=F*P*F'+G*blkdiag(Q1,Q2)*G';
+
+ % Defualt measurement observation matrix and measurement covariance
+ % matrix
+
+ y = in_data.GNSS.pos_ned(:,ctr_gnss_data);
+ H = [eye(3) zeros(3,12)];
+ R = settings.sigma_gps^2*eye(3);
+
+ ind = zeros(1,6); % index vector, describing available measurements
+ % Check if GNSS measurement is available
+ if t(k)==in_data.GNSS.t(ctr_gnss_data)
+ if GPS_position
+ ind(1:3)=1;
+ end
+ % Update GNSS data counter
+ ctr_gnss_data=min(ctr_gnss_data+1,length(in_data.GNSS.t));
+ end
+
+ ind=logical(ind);
+ H=H(ind,:);
+ y=y(ind);
+ R=R(ind,ind);
+
+ % Calculate the Kalman filter gain.
+ K=(P*H')/(H*P*H'+R);
+
+ % Update the perturbation state estimate.
+ z=[zeros(9,1); delta_u_h]+K*(y-H(:,1:6)*x_h(1:6));
+
+ % Correct the navigation states using current perturbation estimates.
+ x_h(1:6)=x_h(1:6)+z(1:6);
+ x_h(7:10)=Gamma(x_h(7:10),z(7:9));
+ delta_u_h=z(10:15);
+
+ % Update the Kalman filter state covariance.
+ P=(eye(15)-K*H)*P;
+
+ % Save the data to the output data structure
+ out_data.x_h(:,k)=x_h;
+ out_data.diag_P(:,k)=diag(P);
+ out_data.delta_u_h(:,k)=delta_u_h;
+
+end
+figure(11)
+plot(a),grid
+energy
+end
+
+
+
+
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%% SUB-FUNCTIONS %%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%% Init filter %%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function [P,Q1,Q2,R,H]=init_filter(settings)
+
+
+% Kalman filter state matrix
+P=zeros(15);
+P(1:3,1:3)=settings.factp(1)^2*eye(3);
+P(4:6,4:6)=settings.factp(2)^2*eye(3);
+P(7:9,7:9)=diag(settings.factp(3:5)).^2;
+P(10:12,10:12)=settings.factp(6)^2*eye(3);
+P(13:15,13:15)=settings.factp(7)^2*eye(3);
+
+% Process noise covariance
+Q1=zeros(6);
+Q1(1:3,1:3)=diag(settings.sigma_acc).^2;
+Q1(4:6,4:6)=diag(settings.sigma_gyro).^2;
+
+Q2=zeros(6);
+Q2(1:3,1:3)=settings.sigma_acc_bias^2*eye(3);
+Q2(4:6,4:6)=settings.sigma_gyro_bias^2*eye(3);
+
+% GNSS-receiver position measurement noise
+R=settings.sigma_gps^2*eye(3);
+
+% Observation matrix
+H=[eye(3) zeros(3,12)];
+
+end
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%% Init navigation state %%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function x_h=init_navigation_state(u,settings)
+
+
+% Calculate the roll and pitch
+f=mean(u(:,1:100),2);
+roll=atan2(-f(2),-f(3));
+pitch=atan2(f(1),norm(f(2:3)));
+
+% Initial coordinate rotation matrix
+Rb2t=Rt2b([roll pitch settings.init_heading])';
+
+% Calculate quaternions
+q=dcm2q(Rb2t);
+
+% Initial navigation state vector
+x_h=[zeros(6,1); q];
+
+end
+
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%% State transition matrix %%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function [F,G]=state_space_model(x,u,Ts)
+
+% Convert quaternion to DCM
+Rb2t=q2dcm(x(7:10));
+
+% Transform measured force to force in
+% the tangent plane coordinate system.
+f_t=Rb2t*u(1:3);
+St=[0 -f_t(3) f_t(2); f_t(3) 0 -f_t(1); -f_t(2) f_t(1) 0];
+
+% Only the standard errors included
+O=zeros(3);
+I=eye(3);
+Fc=[O I O O O;
+ O O St Rb2t O;
+ O O O O -Rb2t;
+ O O O O O;
+ O O O O O];
+
+% Approximation of the discret
+% time state transition matrix
+F=eye(15)+Ts*Fc;
+
+% Noise gain matrix
+G=Ts*[O O O O; Rb2t O O O; O -Rb2t O O; O O I O; O O O I];
+end
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%% Error correction of quaternion %%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function q=Gamma(q,epsilon)
+
+% Convert quaternion to DCM
+R=q2dcm(q);
+
+% Construct skew symetric matrx
+OMEGA=[0 -epsilon(3) epsilon(2); epsilon(3) 0 -epsilon(1); -epsilon(2) epsilon(1) 0];
+
+% Correct the DCM matrix
+R=(eye(3)-OMEGA)*R;
+
+% Calculte the corrected quaternions
+q=dcm2q(R);
+end
+
+
+
+
+
+
+
diff --git a/matlab/Nav_eq.m b/matlab/Nav_eq.m
new file mode 100644
index 0000000000000000000000000000000000000000..3c0a3c4d91b472384714de47a6194ef7a39053ff
--- /dev/null
+++ b/matlab/Nav_eq.m
@@ -0,0 +1,71 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+% function call: x=Nav_eq(x,u,dt)
+%
+% Function that implements the navigation equations of an INS.
+%
+% Inputs:
+% x Current navigation state [position (NED), velocity (NED), attitude (Quaternion)]
+% u Measured inertial quantities [Specific force (m/s^2), Angular velocity (rad/s)]
+% dt Sampling period (s)
+%
+% Output:
+% x Updated navigation state [position (NED), velocity (NED), attitude (Quaternion)]
+%
+% Edit: Isaac Skog (skog@kth.se), 2016-09-06
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function x=Nav_eq(x,u,dt)
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%% Position and velocity %%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+% Gravity vector (This should depend on the current location, but
+% since moving in a small area it is approximatly constant).
+g_t=gravity(59,0);
+
+f_t=q2dcm(x(7:10))*u(1:3);
+
+% Subtract gravity, to obtain accelerations in tangent plane coordinates
+acc_t=f_t-g_t;
+
+% state space model matrices
+A=eye(6);
+A(1,4)=dt;
+A(2,5)=dt;
+A(3,6)=dt;
+
+B=[(dt^2)/2*eye(3);dt*eye(3)];
+
+% Position and velocity update
+x(1:6)=A*x(1:6)+B*acc_t;
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%% Attitude Quaternion %%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Quaternion algorithm according to Farrel, pp 49.
+
+w_tb=u(4:6);
+
+P=w_tb(1)*dt;
+Q=w_tb(2)*dt;
+R=w_tb(3)*dt;
+
+
+OMEGA=zeros(4);
+OMEGA(1,1:4)=0.5*[0 R -Q P];
+OMEGA(2,1:4)=0.5*[-R 0 P Q];
+OMEGA(3,1:4)=0.5*[Q -P 0 R];
+OMEGA(4,1:4)=0.5*[-P -Q -R 0];
+
+v=norm(w_tb)*dt;
+
+if v~=0
+ x(7:10)=(cos(v/2)*eye(4)+2/v*sin(v/2)*OMEGA )*x(7:10);
+end
+return
\ No newline at end of file
diff --git a/matlab/Rt2b.m b/matlab/Rt2b.m
new file mode 100644
index 0000000000000000000000000000000000000000..4fd56a24655a35229655911aede8f438a938af16
--- /dev/null
+++ b/matlab/Rt2b.m
@@ -0,0 +1,21 @@
+function R=Rt2b(ang)
+% function for calculation of the rotation matrix for
+% rotaion from tangent frame to body frame.
+% function R=Rt2b[roll,pitch,yaw];
+% v_b=[u v d]'
+% v_t=[N E D]'
+
+cr=cos(ang(1));
+sr=sin(ang(1));
+
+cp=cos(ang(2));
+sp=sin(ang(2));
+
+cy=cos(ang(3));
+sy=sin(ang(3));
+
+R=[cy*cp sy*cp -sp;
+ -sy*cr+cy*sp*sr cy*cr+sy*sp*sr cp*sr;
+ sy*sr+cy*sp*cr -cy*sr+sy*sp*cr cp*cr];
+
+
diff --git a/matlab/dcm2q.m b/matlab/dcm2q.m
new file mode 100644
index 0000000000000000000000000000000000000000..5a64b2ed9cfd839b2c426b647f2e1a68733d6bdd
--- /dev/null
+++ b/matlab/dcm2q.m
@@ -0,0 +1,19 @@
+function q=dcm2q(R)
+%
+% function q=dcm2q(R)
+% Function for transformation from directional cosine matrix to quaternions
+% From Farrel pp 42.
+% Edit: Isaac Skog, 2007-05-24
+
+
+
+q=zeros(4,1);
+
+q(4)=0.5*sqrt(1+sum(diag(R)));
+
+q(1)=(R(3,2)-R(2,3))/(4*q(4));
+
+q(2)=(R(1,3)-R(3,1))/(4*q(4));
+
+q(3)=(R(2,1)-R(1,2))/(4*q(4));
+
diff --git a/matlab/get_settings.m b/matlab/get_settings.m
new file mode 100644
index 0000000000000000000000000000000000000000..f4946dd13c4a7cfbeb180a9eb158b2099c7f1bc3
--- /dev/null
+++ b/matlab/get_settings.m
@@ -0,0 +1,50 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+% Function that outputs a struct "settings" with the settings used in the
+% GNSS-aided INS
+%
+% Edit: Isaac Skog (skog@kth.se), 2016-09-01
+% Revised: Bo Bernhardsson 2018-01-01
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function settings=get_settings()
+
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%% GENERAL PARAMETERS %%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%settings.non_holonomic = 'off';
+%settings.speed_aiding = 'off';
+
+settings.init_heading = 320*pi/180;
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%% FILTER PARAMETERS %%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+% Process noise covariance (Q)
+% Standard deviations, need to be squared
+settings.sigma_acc = 0.05; % [m/s^2]
+settings.sigma_gyro = 0.1*pi/180; % [rad/s]
+settings.sigma_acc_bias = 0.0001; % [m/s^2.5]
+settings.sigma_gyro_bias = 0.01*pi/180; % [rad/s^1.5]
+
+
+% GNSS position measurement noise covariance (R)
+% Standard deviations, need to be squared
+settings.sigma_gps = 3/sqrt(3); %[m]
+
+% Initial Kalman filter uncertainties (standard deviations)
+settings.factp(1) = 10; % Position [m]
+settings.factp(2) = 5; % Velocity [m/s]
+settings.factp(3:5) = (pi/180*[1 1 20]'); % Attitude (roll,pitch,yaw) [rad]
+settings.factp(6) = 0.02; % Accelerometer biases [m/s^2]
+settings.factp(7) = (0.05*pi/180); % Gyro biases [rad/s]
+
+
+end
+
+
+
+
diff --git a/matlab/gps.m b/matlab/gps.m
new file mode 100644
index 0000000000000000000000000000000000000000..32d14cbbc3638794ea0fe6e39e3ec9ef22bbf9b8
--- /dev/null
+++ b/matlab/gps.m
@@ -0,0 +1,163 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%% GPS sensor dynamics %%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%STATE is a struct (called sensor) with the following fields
+% state -[string]the current proceural state: no_info, cold_start, read_ephemeris, position_available, warm_start_avaialable, warm_start
+% howLong -[double]when the current procedural state has been entered
+% ephExp -[double]expiration time of ephemeris data
+% fp -[int]number of tracked satelliets
+% eph -[int]number of satellites for which ephemeris data are available
+
+%INPUTS
+% -time
+% -state
+% -turn on/off (true=turn_ON, false=turn_OFF)
+% -visible satellites
+
+%OUTPUTS
+% -state
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function sensor=gps(time,sensor_in,turn,sv)
+
+%% parameters
+%define how many satellites you need to track given the accuracy requirements
+required_sv= 4;
+%define how long ephemeris data last
+ephDuration=1800.0;
+%define how much time is required to read the ephemeris data
+ttge=59.0;
+%define how much time is required to fetch the satellites signal
+ttfs=0.01;
+
+%% initialize output
+sensor = sensor_in;
+
+%% equations
+%you cannot track satellites that you dont see
+sensor.fp=min(sensor_in.fp,sv);
+sensor.eph=min(sensor_in.eph,sv);
+
+%% events
+ephemeris_expired=(time>sensor.ephExp)||(sensor.eph<required_sv); %ephemeris data are expired
+lost_visibility=(sensor_in.fp~=sensor.fp)&&(sensor.fp<required_sv);
+fetch_fp=(time-sensor_in.howLong)>=ttfs;
+get_ephemeris=(time-sensor_in.howLong)>ttge;
+
+%% transitions and updates
+%for each of the states handle eventual transitions and updates
+%(i)if any transition is fired, always update the time the surrent state was entered
+%(ii)other updates depend on the spefic transition that is fired
+if strcmp(sensor_in.state,'no_info')
+ if(turn)
+ sensor.state='cold_start';
+ sensor.howLong=time;
+ end
+end
+if strcmp(sensor_in.state,'position_available')
+ if(get_ephemeris)
+ sensor.state='position_available';
+ sensor.howLong=time;
+ sensor.eph=sv;
+ sensor.ephExp=time+ephDuration;
+ end
+ if(~turn)
+ sensor.state='warm_start_available';
+ sensor.howLong=time;
+ sensor.fp=0;
+ end
+ if(lost_visibility)
+ sensor.state='warm_start';
+ sensor.howLong=time;
+ %might have to go on to cold_start
+ if(ephemeris_expired)
+ sensor.state='cold_start';
+ sensor.howLong=time;
+ sensor.eph=0;
+ end
+ end
+ if(ephemeris_expired)
+ sensor.state='read_ephemeris';
+ sensor.howLong=time;
+ sensor.eph=0;
+ %might have to go on to cold_start
+ if(lost_visibility)
+ sensor.state='cold_start';
+ sensor.howLong=time;
+ end
+ end
+end
+if strcmp(sensor_in.state,'read_ephemeris')
+ if(get_ephemeris)
+ sensor.state='position_available';
+ sensor.howLong=time;
+ sensor.eph=sv;
+ sensor.ephExp=time+ephDuration;
+ end
+ if(~turn)
+ sensor.state='no_info';
+ sensor.howLong=time;
+ sensor.fp=0;
+ end
+ if(lost_visibility)
+ sensor.state='cold_start';
+ sensor.howLong=time;
+ end
+end
+if strcmp(sensor_in.state,'warm_start')
+ if(lost_visibility)
+ sensor.state='warm_start';
+ sensor.howLong=time;
+ end
+ if(fetch_fp)
+ sensor.state='position_available';
+ sensor.howLong=time;
+ sensor.fp=sv;
+ end
+ if(ephemeris_expired)
+ sensor.state='cold_start';
+ sensor.howLong=time;
+ sensor.eph=0;
+ end
+ if(~turn)
+ sensor.state='warm_start_available';
+ sensor.howLong=time;
+ sensor.fp=0;
+ end
+end
+if strcmp(sensor_in.state,'cold_start')
+ if(fetch_fp)
+ sensor.state='read_ephemeris';
+ sensor.howLong=time;
+ sensor.fp=sv;
+ end
+ if(lost_visibility)
+ sensor.state='cold_start';
+ sensor.howLong=time;
+ end
+ if(~turn)
+ sensor.state='no_info';
+ sensor.howLong=time;
+ sensor.fp=0;
+ end
+end
+if strcmp(sensor_in.state,'warm_start_available')
+ if(turn)
+ sensor.state='warm_start';
+ sensor.howLong=time;
+ end
+ if(ephemeris_expired)
+ sensor.state='no_info';
+ sensor.howLong=time;
+ sensor.eph=0;
+ end
+end
+
+end
+
+
+
+
+
diff --git a/matlab/gravity.m b/matlab/gravity.m
new file mode 100644
index 0000000000000000000000000000000000000000..d403f400a5d2978136617be65365d0da80f36288
--- /dev/null
+++ b/matlab/gravity.m
@@ -0,0 +1,29 @@
+function g=gravity(lambda,h)
+% function g=gravity(lambda,h)
+%
+% function for calculation of the local gravity vector, in
+% the geographic reference frame (same as tangent plane is
+% stationary).
+%
+% Based upon the WGS_84 Geodetic and Gravity model. For more
+% info see [pp 222-223,1].
+%
+% lambda -> Latitude [degrees]
+% h -> Altitude [m]
+% g
+%
+% edit: Isaac Skog, 2006-08-17
+
+% degrees to radians
+lambda=pi/180*lambda;
+
+gamma=9.780327*(1+0.0053024*sin(lambda)^2-0.0000058*sin(2*lambda)^2);
+
+g=gamma-((3.0877e-6)-(0.004e-6)*sin(lambda)^2)*h+(0.072e-12)*h^2;
+
+g=[0 0 -g]';
+return;
+
+
+
+
diff --git a/matlab/main.m b/matlab/main.m
new file mode 100644
index 0000000000000000000000000000000000000000..1308da4dea72e45e08e222a270f731917ad86f05
--- /dev/null
+++ b/matlab/main.m
@@ -0,0 +1,21 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Main script for pAN_ powe aware navigation with sensor fusion
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+
+%% Load data
+disp('Loads data')
+load('GNSSaidedINS_data.mat');
+
+%% Load filter settings
+disp('Loads settings')
+settings=get_settings();
+
+%% Run the GNSS-aided INS
+disp('Runs the GNSS-aided INS')
+out_data=GPSaidedINS(in_data,settings);
+
+%% Plot the data
+disp('Plot data')
+plot_data(in_data,out_data,'True');drawnow
+
diff --git a/matlab/plot_data.m b/matlab/plot_data.m
new file mode 100644
index 0000000000000000000000000000000000000000..0a37b18d983ecfb633af1f63640fad07059c7b19
--- /dev/null
+++ b/matlab/plot_data.m
@@ -0,0 +1,144 @@
+function plot_data(in_data,out_data,holdon)
+
+if nargin < 3; holdon = 'False'; end
+
+h=zeros(1,3);
+figure(2)
+if ~holdon clf; end
+plot(in_data.GNSS.pos_ned(2,:),in_data.GNSS.pos_ned(1,:),'b-');
+hold on
+h(1)=plot(in_data.GNSS.pos_ned(2,:),in_data.GNSS.pos_ned(1,:),'b.');
+h(2)=plot(out_data.x_h(2,:),out_data.x_h(1,:),'r');
+h(3)=plot(in_data.GNSS.pos_ned(2,1),in_data.GNSS.pos_ned(1,1),'ks');
+title('Trajectory')
+ylabel('North [m]')
+xlabel('East [m]')
+legend(h,'GNSS position estimate','GNSS aided INS trajectory','Start point')
+axis equal
+grid on
+
+h=zeros(1,3);
+figure(3)
+if ~holdon clf; end
+h(1)=plot(in_data.GNSS.t,-in_data.GNSS.pos_ned(3,:),'b.');
+hold on
+h(2)=plot(in_data.IMU.t,-out_data.x_h(3,:),'r');
+h(3)=plot(in_data.IMU.t,3*sqrt(out_data.diag_P(3,:))-out_data.x_h(3,:),'k--');
+plot(in_data.IMU.t,-3*sqrt(out_data.diag_P(3,:))-out_data.x_h(3,:),'k--')
+title('Height versus time')
+ylabel('Height [m]')
+xlabel('Time [s]')
+grid on
+legend(h,'GNSS estimate','GNSS aided INS estimate','3\sigma bound')
+
+h=zeros(1,2);
+figure(4)
+if ~holdon clf; end
+h(1)=plot(in_data.IMU.t,sqrt(sum(out_data.x_h(4:6,:).^2)),'r');
+hold on
+sigma=sqrt(sum(out_data.diag_P(4:6,:)));
+speed=sqrt(sum(out_data.x_h(4:6,:).^2));
+h(2)=plot(in_data.IMU.t,3*sigma+speed,'k--');
+plot(in_data.IMU.t,-3*sigma+speed,'k--')
+title('Speed versus time')
+ylabel('Speed [m/s]')
+xlabel('Time [s]')
+grid on
+legend(h,'GNSS aided INS estimate','3\sigma bound')
+
+N=length(in_data.IMU.t);
+attitude=zeros(3,N);
+
+for n=1:N
+ Rb2t=q2dcm(out_data.x_h(7:10,n));
+
+ %Get the roll, pitch and yaw
+ %roll
+ attitude(1,n)=atan2(Rb2t(3,2),Rb2t(3,3))*180/pi;
+
+ %pitch
+ attitude(2,n)=-atan(Rb2t(3,1)/sqrt(1-Rb2t(3,1)^2))*180/pi;
+
+ %yaw
+ attitude(3,n)=atan2(Rb2t(2,1),Rb2t(1,1))*180/pi;
+end
+
+ylabels={'Roll [deg]','Pitch [deg]','Yaw [deg]'};
+figure(5)
+if ~holdon clf; end
+h=zeros(1,2);
+for k=1:3
+ subplot(3,1,k)
+ h(1)=plot(in_data.IMU.t,attitude(k,:),'r');
+ hold on
+ plot(in_data.IMU.t,3*180/pi*sqrt(out_data.diag_P(6+k,:))+attitude(k,:),'k--')
+ h(2)=plot(in_data.IMU.t,-3*180/pi*sqrt(out_data.diag_P(6+k,:))+attitude(k,:),'k--');
+ grid on
+ ylabel(ylabels{k})
+ if k==1
+ title('Attitude versus time')
+ end
+end
+xlabel('Time [s]')
+legend(h,'GNSS aided INS estimate','3\sigma bound')
+
+
+figure(6)
+if ~holdon clf; end
+ylabels={'X-axis bias [m/s^2]','Y-axis bias [m/s^2]','Z-axis bias [m/s^2]'};
+h=zeros(1,2);
+for k=1:3
+ subplot(3,1,k)
+ h(1)=plot(in_data.IMU.t,out_data.delta_u_h(k,:),'r');
+ hold on
+ h(2)=plot(in_data.IMU.t,3*sqrt(out_data.diag_P(9+k,:))+out_data.delta_u_h(k,:),'k--');
+ plot(in_data.IMU.t,-3*sqrt(out_data.diag_P(9+k,:))+out_data.delta_u_h(k,:),'k--')
+ grid on
+ ylabel(ylabels{k})
+ if k==1
+ title('Accelerometer bias estimate versus time')
+ end
+end
+xlabel('Time [s]')
+legend(h,'GNSS aided INS estimate','3\sigma bound')
+
+
+figure(7)
+if ~holdon clf; end
+ylabels={'X-axis bias [deg/s]','Y-axis bias [deg/s]','Z-axis bias [deg/s]'};
+h=zeros(1,2);
+for k=1:3
+ subplot(3,1,k)
+ h(1)=plot(in_data.IMU.t,180/pi*out_data.delta_u_h(3+k,:),'r');
+ hold on
+ h(2)=plot(in_data.IMU.t,3*180/pi*sqrt(out_data.diag_P(12+k,:))+180/pi*out_data.delta_u_h(3+k,:),'k--');
+ plot(in_data.IMU.t,-3*180/pi*sqrt(out_data.diag_P(12+k,:))+180/pi*out_data.delta_u_h(3+k,:),'k--')
+ grid on
+ ylabel(ylabels{k})
+ if k==1
+ title('Gyroscope bias estimate versus time')
+ end
+end
+xlabel('Time [s]')
+legend(h,'GNSS aided INS estimate','3\sigma bound')
+
+figure(8)
+if ~holdon clf; end
+xest = out_data.x_h(2,:);
+yest = out_data.x_h(1,:);
+xgps = interp1(in_data.GNSS.t,in_data.GNSS.pos_ned(2,:),in_data.IMU.t,'linear','extrap')';
+ygps = interp1(in_data.GNSS.t,in_data.GNSS.pos_ned(1,:),in_data.IMU.t,'linear','extrap')';
+xerr = xest - xgps;
+yerr = yest - ygps;
+plot(in_data.IMU.t,xerr)
+grid on
+hold on
+plot(in_data.IMU.t,yerr)
+xlabel('time [s]')
+ylabel('position difference [m]')
+legend('x', 'y')
+
+positionerr_RMS = sqrt(mean(xerr.^2+yerr.^2))
+figure(2)
+end
+
diff --git a/matlab/q2dcm.m b/matlab/q2dcm.m
new file mode 100644
index 0000000000000000000000000000000000000000..3f36a5d144e27b250e5380a26c5ec79628ef3095
--- /dev/null
+++ b/matlab/q2dcm.m
@@ -0,0 +1,56 @@
+function R=q2dcm(q)
+% function R=q2dcm(q)
+% Function for transformation from quaternions to directional cosine matrix
+% Farell pp.41
+% Edit: Isaac Skog, 2007-05-24
+
+p=zeros(6,1);
+
+p(1:4)=q.^2;
+
+p(5)=p(2)+p(3);
+
+if p(1)+p(4)+p(5)~=0
+ p(6)=2/(p(1)+p(4)+p(5));
+else
+ p(6)=0;
+end
+
+R(1,1)=1-p(6)*p(5);
+R(2,2)=1-p(6)*(p(1)+p(3));
+R(3,3)=1-p(6)*(p(1)+p(2));
+
+p(1)=p(6)*q(1);
+p(2)=p(6)*q(2);
+p(5)=p(6)*q(3)*q(4);
+p(6)=p(1)*q(2);
+
+R(1,2)=p(6)-p(5);
+R(2,1)=p(6)+p(5);
+
+p(5)=p(2)*q(4);
+p(6)=p(1)*q(3);
+
+R(1,3)=p(6)+p(5);
+R(3,1)=p(6)-p(5);
+
+p(5)=p(1)*q(4);
+p(6)=p(2)*q(3);
+
+R(2,3)=p(6)-p(5);
+R(3,2)=p(6)+p(5);
+
+% R(1,2)=2*(q(1)*q(2)-q(3)*q(4));
+% R(1,3)=2*(q(1)*q(3)+q(2)*q(4));
+%
+% % Row 2
+% R(2,1)=2*(q(1)*q(2)+q(3)*q(4));
+% R(2,2)=q(2)^2+q(4)^2-q(1)^2-q(3)^2;
+% R(2,3)=2*(q(2)*q(3)-q(1)*q(4));
+%
+% % Row 3
+% R(3,1)=2*(q(1)*q(3)-q(2)*q(4));
+% R(3,2)=2*(q(2)*q(3)+q(1)*q(4));
+% R(3,3)=q(3)^2+q(4)^2-q(1)^2-q(2)^2;
+%
+% % Okej, men kan f�rb�trras, se pp 41 Farrel
\ No newline at end of file