diff --git a/matlab/display_status.m b/matlab/display_status.m
new file mode 100644
index 0000000000000000000000000000000000000000..20b2d6a40d59ff4780af40a3899d8aedd1cffae6
--- /dev/null
+++ b/matlab/display_status.m
@@ -0,0 +1,3 @@
+function test_cb(src,msg)
+disp('testcb')
+end
diff --git a/matlab/genav_init.png b/matlab/genav_init.png
new file mode 100644
index 0000000000000000000000000000000000000000..e0e72afa92956ac9366fbe1e7cee80d3d3034e67
Binary files /dev/null and b/matlab/genav_init.png differ
diff --git a/matlab/navsat_init.png b/matlab/navsat_init.png
new file mode 100644
index 0000000000000000000000000000000000000000..c305921beddb28ba79137dd13eae91d869698fbf
Binary files /dev/null and b/matlab/navsat_init.png differ
diff --git a/matlab/pioneer_los.m b/matlab/pioneer_los.m
new file mode 100644
index 0000000000000000000000000000000000000000..da19443899831b427886356a02c2441451fff5fc
--- /dev/null
+++ b/matlab/pioneer_los.m
@@ -0,0 +1,142 @@
+function [linvel,angvel] = pioneer_los(x,y,th,xstart,ystart,xend,yend)
+
+% pioneer_los controller
+% Line of sight controller for pioneer
+% Inputs:
+%  x: current x position of robot
+%  y: 
+%  th:
+%  xstart,ystart: starting point of the line
+%  xend,yend: ending point (goal) of the line
+
+
+% Bearing from current location to end point
+angle2end = (atan2((yend-y),(xend-x))*180/pi); %*(-1) + 90
+
+
+anglelinerad = atan2(yend-ystart,xend-xstart);
+
+% Slope of line
+if ((xend-xstart)==0)
+    m = 10000;
+else
+    m = (yend-ystart)/(xend-xstart);
+    
+% Intersection between robot current location and line
+if (m==0)
+    yint = ystart;
+    xint = x;
+elseif (abs(m) > 1000)
+    yint = y;
+    xint = xstart
+else
+    xint = (m/(m^2+1))*(y-ystart+m*xstart+x/m)
+    yint = ystart+m*(xint-xstart)
+end
+
+% Distance to line
+d2line = sqrt((x-xint)**2+(y-yint)**2);
+% msg = "LOS:xint: %5.2f,yint: %5.2f,m: %5.2f"%(xint,yint,m)
+angle2line = atan2(yint-y,xint-x)*180/pi;
+% Calculate the point to aim at
+dist2goal = distance2end(behavior,state);
+if (dist2goal > Ggain['lineDelta'])
+    xtgt = xint+Ggain['lineDelta']*cos(anglelinerad)
+    ytgt = yint+Ggain['lineDelta']*sin(anglelinerad)
+else
+    xtgt = xend
+    ytgt = yend
+end
+    
+%print "%f:%f"%(state[0],state[1])
+%bearing2line = -angle2line + 90.0
+if (m == 0)
+    if (yint > y)
+        GlineInt = GlineInt + d2line*GdT
+    else
+        GlineInt = GlineInt - d2line*GdT
+    end
+elseif (abs(m) > 1000)
+    if (xint > x)
+        GlineInt = GlineInt + d2line*GdT
+    else
+        GlineInt = GlineInt - d2line*GdT
+    end
+else
+    if (angle2line > 0 and angle2line < 180)
+        GlineInt = GlineInt + d2line*GdT
+    else
+        GlineInt = GlineInt - d2line*GdT
+
+GlineInt = saturate(GlineInt, Ggain['lineWindup'])
+if GoldD2line != d2line:
+    GlineDev = (d2line - GoldD2line)/GdT
+end
+
+% set a hard max. on the derivative - this is a hack!
+if abs(GlineDev) > 10.0:
+    GlineDev = 0.0
+GlineDev = saturate(GlineDev,2.0)
+
+%if angle2end < 0:
+%    angle2end = 360 + angle2end
+%if angle2line < 0:
+%    angle2line = 360 + angle2line
+
+if m ==0:
+    if xend > xstart:
+        GlineDev = -GlineDev
+    end
+elseif abs(m) > 1000:
+    if yend < ystart:
+        GlineDev = -GlineDev
+    end
+else
+    if xend < xstart:
+        GlineDev = -GlineDev
+    end
+end
+
+% Calculate angle to the goal
+angleP = atan2(ytgt-state[1],xtgt-state[0])*180/pi
+if angleP < 0:
+    angleP = 360+angleP
+end
+angleD = -Ggain['losKd']*GlineDev
+if angle2end >= angle2line:
+    angleGoal = angleP+angleD
+else
+    angleGoal = angleP-angleD
+end
+
+hdgGoal = -1.0*angleGoal + 90.0
+if hdgGoal < -180:
+    hdgGoal = hdgGoal +360
+end
+if hdgGoal > 180:
+    hdgGoal = hdgGoal - 360
+end
+behavior.behavior_t.heading = hdgGoal
+
+ThrustHdg = HeadingControl(behavior,state)
+% The speed can be set a few ways
+if (behavior.behavior_t.velocity != -1) and (behavior.behavior_t.velocity != None):
+    % slow down when we get close
+    if abs(dist2goal) < 5.0:
+        behavior.behavior_t.velocity = min(behavior.behavior_t.velocity,abs(dist2goal)/5.0)
+    end
+    ThrustVel = VelocityControl(behavior,state)
+    %print "vel control"
+else:
+    ThrustVel = DistanceControl2(behavior,state)
+end
+
+Tport = saturate(ThrustHdg[0]+ThrustVel[0],1.0)
+Tstbd = saturate(ThrustHdg[1]+ThrustVel[1],1.0)
+
+msg ="LOS: dist2goal: %5.1f, dist2line: %5.2f, oldd2line: %5.2f, angle2line %5.1f, hdgGoal=%5.2f, angleGoal=%5.2f, angleP: %5.2f, angleD: %5.3f, GlineDev: %4.7f, Tp=%5.2f, Ts=%5.2f"% \
+(dist2goal,d2line, GoldD2line,angle2line,hdgGoal,angleGoal,angleP,angleD,GlineDev,Tport,Tstbd)
+if GoldD2line != d2line:
+    GoldD2line = d2line
+end
+return [Tport,Tstbd,behavior,msg]
diff --git a/matlab/post_nav_status.m b/matlab/post_nav_status.m
new file mode 100644
index 0000000000000000000000000000000000000000..0a7a10c80f9a5b82a9c07fca617ea705d0023691
--- /dev/null
+++ b/matlab/post_nav_status.m
@@ -0,0 +1,192 @@
+close('all')
+
+% Create a bag file object with the file name
+%bag = rosbag('../data/perimeter_all3.bag')
+bag = rosbag('2017-05-30-20-02-36.bag');
+ 
+% Display a list of the topics and message types in the bag file
+bag.AvailableTopics
+ 
+
+%% Odometry estimate from P3AT
+ns='p1';
+bagselect = select(bag,'Topic',['/' ns '/my_p3at/pose']);
+% Create a time series object based on the fields of the message type
+ts_odom = timeseries(bagselect,'Pose.Pose.Position.X','Pose.Pose.Position.Y',...
+    'Pose.Pose.Orientation.X','Pose.Pose.Orientation.Y',...
+    'Pose.Pose.Orientation.Z','Pose.Pose.Orientation.W',...
+    'Twist.Twist.Linear.X');
+
+% Extract the data as simple vectors.
+x_odom = ts_odom.Data(:,1);
+y_odom = ts_odom.Data(:,2);
+angles = quat2eul([ts_odom.Data(:,6), ts_odom.Data(:,3),ts_odom.Data(:,4),ts_odom.Data(:,5)]);
+yaw_odom = angles(:,1);
+vel_odom=ts_odom.Data(:,7);
+
+figure();
+clf();
+plot(x_odom,y_odom);
+hold on
+plot(x_odom(1),y_odom(1),'go','markerfacecolor','g')
+plot(x_odom(end),y_odom(end),'ro','markerfacecolor','r')
+xlabel('Odom X [m]')
+ylabel('Odom Y [m]')
+title('Raw Odometry Estimate')
+title('Odometry Estimate')
+legend('Path','Start','End')
+axis('equal')
+grid on;
+
+%% Geonav from GPS/IMU
+bagselect = select(bag,'Topic',['/' ns '/geonav_odom']);
+
+% Create a time series object based on the fields of the message type
+ts_nav = timeseries(bagselect,'Pose.Pose.Position.X','Pose.Pose.Position.Y',...
+    'Pose.Pose.Orientation.X','Pose.Pose.Orientation.Y',...
+    'Pose.Pose.Orientation.Z','Pose.Pose.Orientation.W',...
+    'Twist.Twist.Linear.X');
+% Extract the data as simple vectors.
+x = ts_nav.Data(:,1);
+y = ts_nav.Data(:,2);
+angles = quat2eul([ts_nav.Data(:,6), ts_nav.Data(:,3),ts_nav.Data(:,4),ts_nav.Data(:,5)]);
+yaw = angles(:,1);
+vel=ts_nav.Data(:,7);
+
+% Plot the path with endpoints
+figure();
+clf();
+plot(x,y);
+hold on
+plot(x(1),y(1),'go','markerfacecolor','g')
+plot(x(end),y(end),'ro','markerfacecolor','r')
+xlabel('Geonav X [m]')
+ylabel('Geonav Y [m]')
+legend('Path','Start','End')
+axis('equal')
+grid on;
+
+% Quiver plot to check the heading alignment.
+figure();
+clf()
+u = vel.*cos(yaw);
+v = vel.*sin(yaw);
+ii = 1:10:length(x);  % Decimate the data so that it plot only every Nth point.
+quiver(x(ii),y(ii),u(ii)*10,v(ii)*10);
+hold on
+plot(x(1),y(1),'go','markerfacecolor','g')
+plot(x(end),y(end),'ro','markerfacecolor','r')
+xlabel('Geonav X [m]')
+ylabel('Geonav Y [m]')
+axis('equal')
+grid on;
+
+% Extract the covariance estimates from the navigation solution
+% as a measure of navigation quality
+varX = [];
+varY = [];
+varYaw = [];
+msgs = readMessages(bagselect);
+for ii = 1:length(msgs)
+    varX(ii) = msgs{ii}.Pose.Covariance(1);
+    varY(ii) = msgs{ii}.Pose.Covariance(8);
+    varYaw(ii) = msgs{ii}.Pose.Covariance(22);
+end
+
+figure(4);
+clf();
+subplot(3,1,1)
+t0 = ts_nav.Time-ts_nav.Time(1);
+semilogy(t0/60,sqrt(varX),t0/60,sqrt(varY));
+legend('\sigma_x','\sigma_y');
+ylabel('Geonav Pose StdDev [m]')
+grid('on')
+ylim([0.1,100])
+subplot(3,1,2)
+semilogy(t0/60,sqrt(varYaw)*180/pi);
+grid('on')
+ylabel('Geonav Yaw StdDev [deg]')
+
+%% GPS statu
+bagselect = select(bag,'Topic',['/' ns '/gps/fix']);
+msgs = readMessages(bagselect);
+ts_gps = timeseries(bagselect);
+gpsVarX = [];
+gpsVarY = [];
+gpsVarZ = [];
+gpsStatus = []'
+for ii = 1:length(msgs)
+    gpsVarX(ii) = msgs{ii}.PositionCovariance(1);
+    gpsVarY(ii) = msgs{ii}.PositionCovariance(5);
+    gpsVarZ(ii) = msgs{ii}.PositionCovariance(9);
+    gpsStatus(ii) = msgs{ii}.Status.Status;
+end
+
+
+figure(5);
+clf();
+subplot(2,1,1)
+t3 = ts_gps.Time-ts_nav.Time(1);
+semilogy(t3/60,gpsVarX,t3/60,gpsVarY);
+legend('\sigma_x^2','\sigma_y^2');
+ylabel('GPS Position Variance [m^2]')
+grid('on')
+ylim([0.1 10000])
+subplot(2,1,2)
+plot(t3/60,gpsStatus)
+xlabel('Time [s]')
+ylabel('GPS Fix Status')
+grid('on')
+
+
+%% Navigation Sensor Status
+% Plot the status of the navigation solution
+bagstatus = select(bag,'Topic',['/' ns '/nav/status']);
+msgs = readMessages(bagstatus);
+status = []
+for ii = 1:length(msgs)
+    status(ii,:) = msgs{ii}.Data;
+end
+ts_status = timeseries(bagstatus);
+
+figure();
+clf();
+t1 = ts_status.Time - ts_status.Time(1);
+plot(ts_status.Time,status)
+legend('FilterState','DynamicsMode','StatusFlags');
+title('Navigation Kalman Filter Status')
+xlabel('Time [s]')
+ylabel('Value')
+
+figure(4);
+subplot(3,1,3)
+plot(t1/60,status(:,1))
+xlabel('Time [min]')
+ylabel('Microstain Filter Status')
+grid('on')
+
+
+
+%% Compare odometry and geonav estimates
+% Normalize the odometry with the initial geonav estimate
+dyaw = yaw_odom(1)-yaw(1);  % rotation angle for odom frame 
+dx = x_odom-x_odom(1);
+dy = y_odom-y_odom(1);
+x_o = (dx.*cos(dyaw)-dy.*sin(dyaw))+x(1);
+y_o = (dx.*sin(dyaw)+dy.*cos(dyaw))+y(1);
+
+figure();
+clf();
+plot(x_o,y_o,x,y);
+legend('Odometry','Geonav');
+hold on
+plot(x(1),y(1),'go','markerfacecolor','g')
+plot(x(end),y(end),'ro','markerfacecolor','r')
+plot(x_o(1),y_o(1),'go','markerfacecolor','g')
+plot(x_o(end),y_o(end),'ro','markerfacecolor','r')
+grid on;
+axis('equal')
+xlabel('X [m]')
+ylabel('Y [m]')
+
+
diff --git a/matlab/post_odom.m b/matlab/post_odom.m
index 89656a26c6812992189ba8469aff3564841bc3c2..ebd38f93624de41f9ca9973b6b846f3bfe79db16 100644
--- a/matlab/post_odom.m
+++ b/matlab/post_odom.m
@@ -1,7 +1,8 @@
 close('all')
 
 % Create a bag file object with the file name
-bag = rosbag('../data/perimeter_all3.bag')
+%bag = rosbag('../data/perimeter_all3.bag')
+bag = rosbag('2017-05-30-20-02-36.bag');
  
 % Display a list of the topics and message types in the bag file
 bag.AvailableTopics
diff --git a/matlab/rosclass2.m b/matlab/rosclass2.m
new file mode 100644
index 0000000000000000000000000000000000000000..c7e3b5f0e7d84c662d017ad0736f2dda42e5595e
--- /dev/null
+++ b/matlab/rosclass2.m
@@ -0,0 +1,18 @@
+classdef rosclass2 < handle
+    
+    properties
+        ch_sub 
+    end
+    
+    methods
+        function init(obj)
+            obj.ch_sub = rossubscriber('/p5/geonav_odom','nav_msgs/Odometry',@obj.ch_callback);
+        end
+        function ch_callback(obj,src,msg)
+            disp('ch');
+        end
+        
+    end
+    
+end
+
diff --git a/matlab/rosclass_ex2.m b/matlab/rosclass_ex2.m
new file mode 100644
index 0000000000000000000000000000000000000000..ee25f5c3b3fe3070cbd74debae91e7c3edbf5048
--- /dev/null
+++ b/matlab/rosclass_ex2.m
@@ -0,0 +1,9 @@
+
+rosinit;
+
+rc = rosclass2();
+rc.init()
+
+% Publish to test
+
+
diff --git a/matlab/test_cb.m b/matlab/test_cb.m
new file mode 100644
index 0000000000000000000000000000000000000000..20b2d6a40d59ff4780af40a3899d8aedd1cffae6
--- /dev/null
+++ b/matlab/test_cb.m
@@ -0,0 +1,3 @@
+function test_cb(src,msg)
+disp('testcb')
+end