Merge remote-tracking branch 'upstream/master' into eagletree

59 files changed, 4081 insertions, 235 deletions

diff --git a/ROMFS/logging/logconv.m b/ROMFS/logging/logconv.m
index 92ee01413..3750ddae2 100755..100644
--- a/ROMFS/logging/logconv.m
+++ b/ROMFS/logging/logconv.m
@@ -1,224 +1,586 @@
-% This Matlab Script can be used to import the binary logged values of the
-% PX4FMU into data that can be plotted and analyzed.
-
-% Clear everything
-clc
-clear all
-close all
-
-% Set the path to your sysvector.bin file here
-filePath = 'sysvector.bin';
-
-% Work around a Matlab bug (not related to PX4)
-% where timestamps from 1.1.1970 do not allow to
-% read the file's size
-if ismac
-    system('touch -t 201212121212.12 sysvector.bin');
-end
-
-%%%%%%%%%%%%%%%%%%%%%%%
-% SYSTEM VECTOR
-%
-% //All measurements in NED frame
-%
-% uint64_t timestamp; //[us]
-% float gyro[3]; //[rad/s]
-% float accel[3]; //[m/s^2]
-% float mag[3]; //[gauss] 
-% float baro; //pressure [millibar]
-% float baro_alt; //altitude above MSL [meter]
-% float baro_temp; //[degree celcius]
-% float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
-% float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512)
-% float vbat; //battery voltage in [volt]
-% float bat_current - current drawn from battery at this time instant
-% float bat_discharged - discharged energy in mAh
-% float adc[4]; //ADC ports [volt]
-% float local_position[3]; //tangent plane mapping into x,y,z [m]
-% int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
-% float attitude[3]; //pitch, roll, yaw [rad]
-% float rotMatrix[9]; //unitvectors
-% float actuator_control[4]; //unitvector
-% float optical_flow[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
-% float diff_pressure; - pressure difference in millibar
-% float ind_airspeed;
-% float true_airspeed;
-
-% Definition of the logged values
-logFormat{1} = struct('name', 'timestamp',             'bytes', 8, 'array', 1, 'precision', 'uint64',  'machineformat', 'ieee-le.l64');
-logFormat{2} = struct('name', 'gyro',                  'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{3} = struct('name', 'accel',                 'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{4} = struct('name', 'mag',                   'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{5} = struct('name', 'baro',                  'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{6} = struct('name', 'baro_alt',              'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{7} = struct('name', 'baro_temp',             'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{8} = struct('name', 'control',               'bytes', 4, 'array', 4, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{9} = struct('name', 'actuators',             'bytes', 4, 'array', 8, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{10} = struct('name', 'vbat',                 'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{11} = struct('name', 'bat_current',          'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{12} = struct('name', 'bat_discharged',       'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{13} = struct('name', 'adc',                  'bytes', 4, 'array', 4, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{14} = struct('name', 'local_position',       'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{15} = struct('name', 'gps_raw_position',     'bytes', 4, 'array', 3, 'precision', 'uint32',  'machineformat', 'ieee-le');
-logFormat{16} = struct('name', 'attitude',             'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{17} = struct('name', 'rot_matrix',           'bytes', 4, 'array', 9, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{18} = struct('name', 'vicon_position',       'bytes', 4, 'array', 6, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{19} = struct('name', 'actuator_control',     'bytes', 4, 'array', 4, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{20} = struct('name', 'optical_flow',         'bytes', 4, 'array', 6, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{21} = struct('name', 'diff_pressure',        'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{22} = struct('name', 'ind_airspeed',         'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{23} = struct('name', 'true_airspeed',        'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');
-
-% First get length of one line
-columns = length(logFormat);
-lineLength = 0;
-
-for i=1:columns
-    lineLength = lineLength + logFormat{i}.bytes * logFormat{i}.array;
-end
-
-
-if exist(filePath, 'file')
-    
-    fileInfo = dir(filePath);
-    fileSize = fileInfo.bytes;
-    
-    elements = int64(fileSize./(lineLength));
-    
-    fid = fopen(filePath, 'r');
-    offset = 0;
-    for i=1:columns
-        % using fread with a skip speeds up the import drastically, do not
-        % import the values one after the other
-        sysvector.(genvarname(logFormat{i}.name)) = transpose(fread(...
-            fid, ...
-            [logFormat{i}.array, elements], [num2str(logFormat{i}.array),'*',logFormat{i}.precision,'=>',logFormat{i}.precision], ...
-            lineLength - logFormat{i}.bytes*logFormat{i}.array, ...
-            logFormat{i}.machineformat) ...
-        );
-        offset = offset + logFormat{i}.bytes*logFormat{i}.array;
-        fseek(fid, offset,'bof');
-    end
-    
-    % shot the flight time
-    time_us = sysvector.timestamp(end) - sysvector.timestamp(1);
-    time_s = time_us*1e-6;
-    time_m = time_s/60;
-    
-    % close the logfile
-    fclose(fid);
-    
-    disp(['end log2matlab conversion' char(10)]);
-else
-    disp(['file: ' filePath ' does not exist' char(10)]);
-end
-
-%% Plot GPS RAW measurements
-
-% Only plot GPS data if available
-if cumsum(double(sysvector.gps_raw_position(200:end,1))) > 0
-    figure('units','normalized','outerposition',[0 0 1 1])
-    plot3(sysvector.gps_raw_position(200:end,1), sysvector.gps_raw_position(200:end,2), sysvector.gps_raw_position(200:end,3));
-end
-
-
-%% Plot optical flow trajectory
-
-flow_sz = size(sysvector.timestamp);
-flow_elements = flow_sz(1);
-
-xt(1:flow_elements,1) = sysvector.timestamp(:,1); % time column [ms]
-
-
-%calc dt
-dt = zeros(flow_elements,1);
-for i = 1:flow_elements-1
-    dt(i+1,1) = double(xt(i+1,1)-xt(i,1)) * 10^(-6);   % timestep [s]
-end
-dt(1,1) = mean(dt);
-
-
-global_speed = zeros(flow_elements,3);
-
-%calc global speed (with rot matrix)
-for i = 1:flow_elements
-    rotM = [sysvector.rot_matrix(i,1:3);sysvector.rot_matrix(i,4:6);sysvector.rot_matrix(i,7:9)]';
-    speedX = sysvector.optical_flow(i,3);
-    speedY = sysvector.optical_flow(i,4);
-    
-    relSpeed = [-speedY,speedX,0];
-    global_speed(i,:) = relSpeed * rotM;
-end
-
-
-
-px = zeros(flow_elements,1);
-py = zeros(flow_elements,1);
-distance = 0;
-
-last_vx = 0;
-last_vy = 0;
-elem_cnt = 0;
-
-% Very basic accumulation, stops on bad flow quality
-for i = 1:flow_elements
-    if sysvector.optical_flow(i,6) > 5
-        px(i,1) = global_speed(i,1)*dt(i,1);
-        py(i,1) = global_speed(i,2)*dt(i,1);
-        distance = distance + norm([px(i,1) py(i,1)]);
-        last_vx = px(i,1);
-        last_vy = py(i,1);
-    else
-        px(i,1) = last_vx;
-        py(i,1) = last_vy;
-        last_vx = last_vx*0.95;
-        last_vy = last_vy*0.95;
-    end
-end
-
-px_sum = cumsum(px);
-py_sum = cumsum(py);
-time = cumsum(dt);
-
-figure()
-set(gca, 'Units','normal');
-
-plot(py_sum, px_sum, '-blue', 'LineWidth',2);
-axis equal;
-% set title and axis captions
-xlabel('X position (meters)','fontsize',14)
-ylabel('Y position (meters)','fontsize',14)
-% mark begin and end
-hold on
-plot(py_sum(1,1),px_sum(1,1),'ks','LineWidth',2,...
-'MarkerEdgeColor','k',...
-'MarkerFaceColor','g',...
-'MarkerSize',10)
-hold on
-plot(py_sum(end,1),px_sum(end,1),'kv','LineWidth',2,...
-'MarkerEdgeColor','k',...
-'MarkerFaceColor','b',...
-'MarkerSize',10)
-% add total length as annotation
-set(gca,'fontsize',13);
-legend('Trajectory', 'START', sprintf('END\n(%.2f m, %.0f:%.0f s)', distance, time_m, time_s - time_m*60));
-title('Optical Flow Position Integration', 'fontsize', 15);
-
-figure()
-plot(time, sysvector.optical_flow(:,5), 'blue');
-axis([time(1,1) time(end,1) 0 (max(sysvector.optical_flow(i,5))+0.2)]);
-xlabel('seconds','fontsize',14);
-ylabel('m','fontsize',14);
-set(gca,'fontsize',13);
-title('Ultrasound Altitude', 'fontsize', 15);
-
-
-figure()
-plot(time, global_speed(:,2), 'red');
-hold on;
-plot(time, global_speed(:,1), 'blue');
-legend('y velocity (m/s)', 'x velocity (m/s)');
-xlabel('seconds','fontsize',14);
-ylabel('m/s','fontsize',14);
-set(gca,'fontsize',13);
-title('Optical Flow Velocity', 'fontsize', 15);
+% This Matlab Script can be used to import the binary logged values of the

+% PX4FMU into data that can be plotted and analyzed.

+

+%% ************************************************************************

+% PX4LOG_PLOTSCRIPT: Main function

+% ************************************************************************

+function PX4Log_Plotscript

+

+% Clear everything

+clc

+clear all

+close all

+

+% ************************************************************************

+% SETTINGS

+% ************************************************************************

+

+% Set the path to your sysvector.bin file here

+filePath = 'sysvector.bin';

+

+% Set the minimum and maximum times to plot here [in seconds]

+mintime=0;        %The minimum time/timestamp to display, as set by the user [0 for first element / start]

+maxtime=0;        %The maximum time/timestamp to display, as set by the user [0 for last element / end]

+

+%Determine which data to plot. Not completely implemented yet.

+bDisplayGPS=true;

+

+%conversion factors

+fconv_gpsalt=1E-3; %[mm] to [m]

+fconv_gpslatlong=1E-7; %[gps_raw_position_unit] to [deg]

+fconv_timestamp=1E-6; % [microseconds] to [seconds]

+

+% ************************************************************************

+% Import the PX4 logs

+% ************************************************************************

+ImportPX4LogData();

+

+%Translate min and max plot times to indices

+time=double(sysvector.timestamp) .*fconv_timestamp;

+mintime_log=time(1);        %The minimum time/timestamp found in the log

+maxtime_log=time(end);      %The maximum time/timestamp found in the log

+CurTime=mintime_log;        %The current time at which to draw the aircraft position

+

+[imintime,imaxtime]=FindMinMaxTimeIndices();

+

+% ************************************************************************

+% PLOT & GUI SETUP

+% ************************************************************************

+NrFigures=5;

+NrAxes=10;

+h.figures(1:NrFigures)=0.0;  % Temporary initialization of figure handle array - these are numbered consecutively

+h.axes(1:NrAxes)=0.0;        % Temporary initialization of axes handle array - these are numbered consecutively

+h.pathpoints=[];             % Temporary initiliazation of path points

+

+% Setup the GUI to control the plots

+InitControlGUI();

+% Setup the plotting-GUI (figures, axes) itself.

+InitPlotGUI();

+

+% ************************************************************************

+% DRAW EVERYTHING

+% ************************************************************************

+DrawRawData();

+DrawCurrentAircraftState();

+

+%% ************************************************************************

+%  *** END OF MAIN SCRIPT ***

+%  NESTED FUNCTION DEFINTIONS FROM HERE ON

+%  ************************************************************************

+

+

+%% ************************************************************************

+%  IMPORTPX4LOGDATA (nested function)

+%  ************************************************************************

+%  Attention:  This is the import routine for firmware from ca. 03/2013.

+%              Other firmware versions might require different import 

+%              routines.

+

+function ImportPX4LogData()

+    % Work around a Matlab bug (not related to PX4)

+    % where timestamps from 1.1.1970 do not allow to

+    % read the file's size

+    if ismac

+        system('touch -t 201212121212.12 sysvector.bin');

+    end

+

+    % ************************************************************************

+    % RETRIEVE SYSTEM VECTOR

+    % *************************************************************************

+    % //All measurements in NED frame

+    %

+    % uint64_t timestamp; //[us]

+    % float gyro[3]; //[rad/s]

+    % float accel[3]; //[m/s^2]

+    % float mag[3]; //[gauss] 

+    % float baro; //pressure [millibar]

+    % float baro_alt; //altitude above MSL [meter]

+    % float baro_temp; //[degree celcius]

+    % float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1]

+    % float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512)

+    % float vbat; //battery voltage in [volt]

+    % float bat_current - current drawn from battery at this time instant

+    % float bat_discharged - discharged energy in mAh

+    % float adc[4]; //remaining auxiliary ADC ports [volt]

+    % float local_position[3]; //tangent plane mapping into x,y,z [m]

+    % int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]

+    % float attitude[3]; //pitch, roll, yaw [rad]

+    % float rotMatrix[9]; //unitvectors

+    % float actuator_control[4]; //unitvector

+    % float optical_flow[4]; //roll, pitch, yaw [-1..1], thrust [0..1]

+    % float diff_pressure; - pressure difference in millibar

+    % float ind_airspeed;

+    % float true_airspeed;

+

+    % Definition of the logged values

+    logFormat{1} = struct('name', 'timestamp',             'bytes', 8, 'array', 1, 'precision', 'uint64',  'machineformat', 'ieee-le.l64');

+    logFormat{2} = struct('name', 'gyro',                  'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');

+    logFormat{3} = struct('name', 'accel',                 'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');

+    logFormat{4} = struct('name', 'mag',                   'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');

+    logFormat{5} = struct('name', 'baro',                  'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');

+    logFormat{6} = struct('name', 'baro_alt',              'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');

+    logFormat{7} = struct('name', 'baro_temp',             'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');

+    logFormat{8} = struct('name', 'control',               'bytes', 4, 'array', 4, 'precision', 'float',   'machineformat', 'ieee-le');

+    logFormat{9} = struct('name', 'actuators',             'bytes', 4, 'array', 8, 'precision', 'float',   'machineformat', 'ieee-le');

+    logFormat{10} = struct('name', 'vbat',                 'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');

+    logFormat{11} = struct('name', 'bat_current',          'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');

+    logFormat{12} = struct('name', 'bat_discharged',       'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');

+    logFormat{13} = struct('name', 'adc',                  'bytes', 4, 'array', 4, 'precision', 'float',   'machineformat', 'ieee-le');

+    logFormat{14} = struct('name', 'local_position',       'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');

+    logFormat{15} = struct('name', 'gps_raw_position',     'bytes', 4, 'array', 3, 'precision', 'uint32',  'machineformat', 'ieee-le');

+    logFormat{16} = struct('name', 'attitude',             'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');

+    logFormat{17} = struct('name', 'rot_matrix',           'bytes', 4, 'array', 9, 'precision', 'float',   'machineformat', 'ieee-le');

+    logFormat{18} = struct('name', 'vicon_position',       'bytes', 4, 'array', 6, 'precision', 'float',   'machineformat', 'ieee-le');

+    logFormat{19} = struct('name', 'actuator_control',     'bytes', 4, 'array', 4, 'precision', 'float',   'machineformat', 'ieee-le');

+    logFormat{20} = struct('name', 'optical_flow',         'bytes', 4, 'array', 6, 'precision', 'float',   'machineformat', 'ieee-le');

+    logFormat{21} = struct('name', 'diff_pressure',        'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');

+    logFormat{22} = struct('name', 'ind_airspeed',         'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');

+    logFormat{23} = struct('name', 'true_airspeed',        'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');

+

+    % First get length of one line

+    columns = length(logFormat);

+    lineLength = 0;

+

+    for i=1:columns

+        lineLength = lineLength + logFormat{i}.bytes * logFormat{i}.array;

+    end

+

+

+    if exist(filePath, 'file')

+

+        fileInfo = dir(filePath);

+        fileSize = fileInfo.bytes;

+

+        elements = int64(fileSize./(lineLength));

+

+        fid = fopen(filePath, 'r');

+        offset = 0;

+        for i=1:columns

+            % using fread with a skip speeds up the import drastically, do not

+            % import the values one after the other

+            sysvector.(genvarname(logFormat{i}.name)) = transpose(fread(...

+                fid, ...

+                [logFormat{i}.array, elements], [num2str(logFormat{i}.array),'*',logFormat{i}.precision,'=>',logFormat{i}.precision], ...

+                lineLength - logFormat{i}.bytes*logFormat{i}.array, ...

+                logFormat{i}.machineformat) ...

+            );

+            offset = offset + logFormat{i}.bytes*logFormat{i}.array;

+            fseek(fid, offset,'bof');

+        end

+

+        % shot the flight time

+        time_us = sysvector.timestamp(end) - sysvector.timestamp(1);

+        time_s = time_us*1e-6;

+        time_m = time_s/60;

+

+        % close the logfile

+        fclose(fid);

+

+        disp(['end log2matlab conversion' char(10)]);

+    else

+        disp(['file: ' filePath ' does not exist' char(10)]);

+    end

+end

+

+%% ************************************************************************

+%  INITCONTROLGUI (nested function)

+%  ************************************************************************

+%Setup central control GUI components to control current time where data is shown

+function InitControlGUI()

+    %**********************************************************************

+    % GUI size definitions

+    %**********************************************************************

+    dxy=5; %margins

+    %Panel: Plotctrl

+    dlabels=120;

+    dsliders=200;

+    dedits=80;

+    hslider=20;

+        

+    hpanel1=40; %panel1

+    hpanel2=220;%panel2

+    hpanel3=3*hslider+4*dxy+3*dxy;%panel3.

+    

+    width=dlabels+dsliders+dedits+4*dxy+2*dxy;  %figure width

+    height=hpanel1+hpanel2+hpanel3+4*dxy;       %figure height

+        

+    %**********************************************************************

+    % Create GUI

+    %**********************************************************************

+    h.figures(1)=figure('Units','pixels','position',[200 200 width height],'Name','Control GUI');

+    h.guistatepanel=uipanel('Title','Current GUI state','Units','pixels','Position',[dxy dxy width-2*dxy hpanel1],'parent',h.figures(1));

+    h.aircraftstatepanel=uipanel('Title','Current aircraft state','Units','pixels','Position',[dxy hpanel1+2*dxy width-2*dxy hpanel2],'parent',h.figures(1));

+    h.plotctrlpanel=uipanel('Title','Plot Control','Units','pixels','Position',[dxy hpanel1+hpanel2+3*dxy width-2*dxy hpanel3],'parent',h.figures(1));

+    

+    %%Control GUI-elements

+    %Slider: Current time

+    h.labels.CurTime=uicontrol(gcf,'style','text','Position',[dxy dxy dlabels hslider],'String','Current time t[s]:','parent',h.plotctrlpanel,'HorizontalAlignment','left');

+    h.sliders.CurTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels dxy dsliders hslider],...

+        'min',mintime,'max',maxtime,'value',mintime,'callback',@curtime_callback,'parent',h.plotctrlpanel);

+    temp=get(h.sliders.CurTime,'Max')-get(h.sliders.CurTime,'Min');

+    set(h.sliders.CurTime,'SliderStep',[1.0/temp 5.0/temp]);

+    h.edits.CurTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders dxy dedits hslider],'String',get(h.sliders.CurTime,'value'),...

+        'BackgroundColor','white','callback',@curtime_callback,'parent',h.plotctrlpanel);

+

+    %Slider: MaxTime

+    h.labels.MaxTime=uicontrol(gcf,'style','text','position',[dxy 2*dxy+hslider dlabels hslider],'String','Max. time t[s] to display:','parent',h.plotctrlpanel,'HorizontalAlignment','left');

+    h.sliders.MaxTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 2*dxy+hslider dsliders hslider],...

+        'min',mintime_log,'max',maxtime_log,'value',maxtime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel);

+    h.edits.MaxTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 2*dxy+hslider dedits hslider],'String',get(h.sliders.MaxTime,'value'),...

+        'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel);

+    

+    %Slider: MinTime

+    h.labels.MinTime=uicontrol(gcf,'style','text','position',[dxy 3*dxy+2*hslider dlabels hslider],'String','Min. time t[s] to dispay :','parent',h.plotctrlpanel,'HorizontalAlignment','left');

+    h.sliders.MinTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 3*dxy+2*hslider dsliders hslider],...

+        'min',mintime_log,'max',maxtime_log,'value',mintime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel);

+    h.edits.MinTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 3*dxy+2*hslider dedits hslider],'String',get(h.sliders.MinTime,'value'),...

+        'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel);

+

+    %%Current data/state GUI-elements (Multiline-edit-box)

+    h.edits.AircraftState=uicontrol(gcf,'style','edit','Units','normalized','position',[.02 .02 0.96 0.96],'Min',1,'Max',10,'String','This shows the current aircraft state',...

+                                    'HorizontalAlignment','left','parent',h.aircraftstatepanel);

+    

+    h.labels.GUIState=uicontrol(gcf,'style','text','Units','pixels','position',[dxy dxy width-4*dxy hslider],'String','Current state of this GUI',...

+                                'HorizontalAlignment','left','parent',h.guistatepanel);

+    

+end

+

+%% ************************************************************************

+%  INITPLOTGUI (nested function)

+%  ************************************************************************

+function InitPlotGUI()

+    

+    % Setup handles to lines and text

+    h.markertext=[];

+    templinehandle=0.0;%line([0 1],[0 5]);   % Just a temporary handle to init array

+    h.markerline(1:NrAxes)=templinehandle;   % the actual handle-array to the lines  - these are numbered consecutively

+    h.markerline(1:NrAxes)=0.0;

+        

+    % Setup all other figures and axes for plotting

+    %  PLOT WINDOW 1: GPS POSITION

+    h.figures(2)=figure('units','normalized','Toolbar','figure', 'Name', 'GPS Position');

+    h.axes(1)=axes();

+    set(h.axes(1),'Parent',h.figures(2));

+

+    %  PLOT WINDOW 2: IMU, baro altitude

+    h.figures(3)=figure('Name', 'IMU / Baro Altitude');

+    h.axes(2)=subplot(4,1,1);

+    h.axes(3)=subplot(4,1,2);

+    h.axes(4)=subplot(4,1,3);

+    h.axes(5)=subplot(4,1,4);

+    set(h.axes(2:5),'Parent',h.figures(3));

+    

+    %  PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,...

+    h.figures(4)=figure('Name', 'Attitude Estimate / Actuators / Airspeeds');

+    h.axes(6)=subplot(4,1,1);

+    h.axes(7)=subplot(4,1,2);

+    h.axes(8)=subplot(4,1,3);

+    h.axes(9)=subplot(4,1,4);

+    set(h.axes(6:9),'Parent',h.figures(4));

+    

+    %  PLOT WINDOW 4: LOG STATS

+    h.figures(5) = figure('Name', 'Log Statistics');

+    h.axes(10)=subplot(1,1,1);

+    set(h.axes(10:10),'Parent',h.figures(5));

+    

+end

+

+%% ************************************************************************

+%  DRAWRAWDATA (nested function)

+%  ************************************************************************

+%Draws the raw data from the sysvector, but does not add any

+%marker-lines or so

+function DrawRawData() 

+    % ************************************************************************

+    %  PLOT WINDOW 1: GPS POSITION & GUI

+    %  ************************************************************************

+    figure(h.figures(2));

+    % Only plot GPS data if available

+    if (sum(double(sysvector.gps_raw_position(imintime:imaxtime,1)))>0) && (bDisplayGPS)

+        %Draw data

+        plot3(h.axes(1),double(sysvector.gps_raw_position(imintime:imaxtime,1))*fconv_gpslatlong, ...

+                        double(sysvector.gps_raw_position(imintime:imaxtime,2))*fconv_gpslatlong, ...

+                        double(sysvector.gps_raw_position(imintime:imaxtime,3))*fconv_gpsalt,'r.');

+        title(h.axes(1),'GPS Position Data(if available)');

+        xlabel(h.axes(1),'Latitude [deg]');

+        ylabel(h.axes(1),'Longitude [deg]');

+        zlabel(h.axes(1),'Altitude above MSL [m]');

+        grid on

+        

+        %Reset path

+        h.pathpoints=0;

+    end

+

+    % ************************************************************************

+    %  PLOT WINDOW 2: IMU, baro altitude

+    %  ************************************************************************

+    figure(h.figures(3));

+    plot(h.axes(2),time(imintime:imaxtime),sysvector.mag(imintime:imaxtime,:));

+    title(h.axes(2),'Magnetometers [Gauss]');

+    legend(h.axes(2),'x','y','z');

+    plot(h.axes(3),time(imintime:imaxtime),sysvector.accel(imintime:imaxtime,:));

+    title(h.axes(3),'Accelerometers [m/s²]');

+    legend(h.axes(3),'x','y','z');

+    plot(h.axes(4),time(imintime:imaxtime),sysvector.gyro(imintime:imaxtime,:));

+    title(h.axes(4),'Gyroscopes [rad/s]');

+    legend(h.axes(4),'x','y','z');

+    plot(h.axes(5),time(imintime:imaxtime),sysvector.baro_alt(imintime:imaxtime),'color','blue');

+    if(bDisplayGPS)

+        hold on;

+        plot(h.axes(5),time(imintime:imaxtime),double(sysvector.gps_raw_position(imintime:imaxtime,3)).*fconv_gpsalt,'color','red');

+        hold off

+        legend('Barometric Altitude [m]','GPS Altitude [m]');

+    else

+        legend('Barometric Altitude [m]');

+    end

+    title(h.axes(5),'Altitude above MSL [m]');

+

+    % ************************************************************************

+    %  PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,...

+    % ************************************************************************

+    figure(h.figures(4));

+    %Attitude Estimate

+    plot(h.axes(6),time(imintime:imaxtime), sysvector.attitude(imintime:imaxtime,:).*180./3.14159);

+    title(h.axes(6),'Estimated attitude [deg]');

+    legend(h.axes(6),'roll','pitch','yaw');

+    %Actuator Controls

+    plot(h.axes(7),time(imintime:imaxtime), sysvector.actuator_control(imintime:imaxtime,:));

+    title(h.axes(7),'Actuator control [-]');

+    legend(h.axes(7),'0','1','2','3');

+    %Actuator Controls

+    plot(h.axes(8),time(imintime:imaxtime), sysvector.actuators(imintime:imaxtime,1:8));

+    title(h.axes(8),'Actuator PWM (raw-)outputs [µs]');

+    legend(h.axes(8),'CH1','CH2','CH3','CH4','CH5','CH6','CH7','CH8');

+    set(h.axes(8), 'YLim',[800 2200]);

+    %Airspeeds

+    plot(h.axes(9),time(imintime:imaxtime), sysvector.ind_airspeed(imintime:imaxtime));

+    hold on

+    plot(h.axes(9),time(imintime:imaxtime), sysvector.true_airspeed(imintime:imaxtime));

+    hold off

+    %add GPS total airspeed here

+    title(h.axes(9),'Airspeed [m/s]');

+    legend(h.axes(9),'Indicated Airspeed (IAS)','True Airspeed (TAS)','GPS Airspeed');

+    %calculate time differences and plot them

+    intervals = zeros(0,imaxtime - imintime);

+    for k = imintime+1:imaxtime

+        intervals(k) = time(k) - time(k-1);

+    end

+    plot(h.axes(10), time(imintime:imaxtime), intervals);

+

+    %Set same timescale for all plots

+    for i=2:NrAxes

+        set(h.axes(i),'XLim',[mintime maxtime]);

+    end

+    

+    set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]);

+end

+

+%% ************************************************************************

+%  DRAWCURRENTAIRCRAFTSTATE(nested function)

+%  ************************************************************************

+function DrawCurrentAircraftState()

+    %find current data index        

+    i=find(time>=CurTime,1,'first');

+

+    %**********************************************************************

+    % Current aircraft state label update

+    %**********************************************************************

+    acstate{1,:}=[sprintf('%s \t\t','GPS Pos:'),'[lat=',num2str(double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong),'°, ',...

+                        'lon=',num2str(double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong),'°, ',...

+                        'alt=',num2str(double(sysvector.gps_raw_position(i,3))*fconv_gpsalt),'m]'];

+    acstate{2,:}=[sprintf('%s \t\t','Mags[gauss]'),'[x=',num2str(sysvector.mag(i,1)),...

+                               ', y=',num2str(sysvector.mag(i,2)),...

+                               ', z=',num2str(sysvector.mag(i,3)),']'];

+    acstate{3,:}=[sprintf('%s \t\t','Accels[m/s²]'),'[x=',num2str(sysvector.accel(i,1)),...

+                               ', y=',num2str(sysvector.accel(i,2)),...

+                               ', z=',num2str(sysvector.accel(i,3)),']'];

+    acstate{4,:}=[sprintf('%s \t\t','Gyros[rad/s]'),'[x=',num2str(sysvector.gyro(i,1)),...

+                               ', y=',num2str(sysvector.gyro(i,2)),...

+                               ', z=',num2str(sysvector.gyro(i,3)),']'];

+    acstate{5,:}=[sprintf('%s \t\t','Altitude[m]'),'[Barometric: ',num2str(sysvector.baro_alt(i)),'m, GPS: ',num2str(double(sysvector.gps_raw_position(i,3))*fconv_gpsalt),'m]'];

+    acstate{6,:}=[sprintf('%s \t','Est. attitude[deg]:'),'[Roll=',num2str(sysvector.attitude(i,1).*180./3.14159),...

+                               ', Pitch=',num2str(sysvector.attitude(i,2).*180./3.14159),...

+                               ', Yaw=',num2str(sysvector.attitude(i,3).*180./3.14159),']'];

+    acstate{7,:}=sprintf('%s \t[','Actuator Ctrls [-]:');

+    for j=1:4

+        acstate{7,:}=[acstate{7,:},num2str(sysvector.actuator_control(i,j)),','];

+    end

+    acstate{7,:}=[acstate{7,:},']'];

+    acstate{8,:}=sprintf('%s \t[','Actuator Outputs [PWM/µs]:');

+    for j=1:8

+        acstate{8,:}=[acstate{8,:},num2str(sysvector.actuators(i,j)),','];

+    end

+    acstate{8,:}=[acstate{8,:},']'];

+    acstate{9,:}=[sprintf('%s \t','Airspeed[m/s]:'),'[IAS: ',num2str(sysvector.ind_airspeed(i)),', TAS: ',num2str(sysvector.true_airspeed(i)),']'];

+    

+    set(h.edits.AircraftState,'String',acstate);

+    

+    %**********************************************************************

+    % GPS Plot Update

+    %**********************************************************************

+    %Plot traveled path, and  and time.

+    figure(h.figures(2));

+    hold on;

+    if(CurTime>mintime+1) %the +1 is only a small bugfix

+        h.pathline=plot3(h.axes(1),double(sysvector.gps_raw_position(imintime:i,1))*fconv_gpslatlong, ...

+                                    double(sysvector.gps_raw_position(imintime:i,2))*fconv_gpslatlong, ...

+                                    double(sysvector.gps_raw_position(imintime:i,3))*fconv_gpsalt,'b','LineWidth',2); 

+    end;

+    hold off

+    %Plot current position

+    newpoint=[double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong double(sysvector.gps_raw_position(i,3))*fconv_gpsalt];

+    if(numel(h.pathpoints)<=3) %empty path

+        h.pathpoints(1,1:3)=newpoint;

+    else %Not empty, append new point 

+        h.pathpoints(size(h.pathpoints,1)+1,:)=newpoint;

+    end

+    axes(h.axes(1));    

+    line(h.pathpoints(:,1),h.pathpoints(:,2),h.pathpoints(:,3),'LineStyle','none','Marker','.','MarkerEdge','black','MarkerSize',20);

+    

+    

+    % Plot current time (small label next to current gps position) 

+    textdesc=strcat('  t=',num2str(time(i)),'s');

+    if(isvalidhandle(h.markertext))

+            delete(h.markertext); %delete old text

+    end 

+    h.markertext=text(double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong,double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong,...

+                        double(sysvector.gps_raw_position(i,3))*fconv_gpsalt,textdesc);

+    set(h.edits.CurTime,'String',CurTime);

+        

+    %**********************************************************************

+    % Plot the lines showing the current time in the 2-d plots

+    %**********************************************************************

+    for i=2:NrAxes

+        if(isvalidhandle(h.markerline(i))) delete(h.markerline(i)); end

+        ylims=get(h.axes(i),'YLim');

+        h.markerline(i)=line([CurTime CurTime] ,get(h.axes(i),'YLim'),'Color','black');

+        set(h.markerline(i),'parent',h.axes(i));

+    end

+    

+    set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]);

+end

+

+%% ************************************************************************

+%  MINMAXTIME CALLBACK (nested function)

+%  ************************************************************************

+function minmaxtime_callback(hObj,event) %#ok<INUSL>

+    new_mintime=get(h.sliders.MinTime,'Value');

+    new_maxtime=get(h.sliders.MaxTime,'Value');

+

+    %Safety checks:

+    bErr=false;

+    %1: mintime must be < maxtime

+    if((new_mintime>maxtime) || (new_maxtime<mintime))

+        set(h.labels.GUIState,'String','Error: Mintime cannot be bigger than maxtime! Values were not changed.','BackgroundColor','red');

+        bErr=true;

+    else

+        %2: MinTime must be <=CurTime

+        if(new_mintime>CurTime)

+            set(h.labels.GUIState,'String','Error: Mintime cannot be bigger than CurTime! CurTime set to new mintime.','BackgroundColor','red');

+            mintime=new_mintime;

+            CurTime=mintime;

+            bErr=true;

+        end

+        %3: MaxTime must be >CurTime

+        if(new_maxtime<CurTime)

+            set(h.labels.GUIState,'String','Error: Maxtime cannot be smaller than CurTime! CurTime set to new maxtime.','BackgroundColor','red');

+            maxtime=new_maxtime;

+            CurTime=maxtime;

+            bErr=true;

+        end

+    end

+

+    if(bErr==false)

+        maxtime=new_maxtime;

+        mintime=new_mintime;

+    end

+

+    %Needs to be done in case values were reset above

+    set(h.sliders.MinTime,'Value',mintime);

+    set(h.sliders.MaxTime,'Value',maxtime);

+

+    %Update curtime-slider

+    set(h.sliders.CurTime,'Value',CurTime);

+    set(h.sliders.CurTime,'Max',maxtime);

+    set(h.sliders.CurTime,'Min',mintime);

+    temp=get(h.sliders.CurTime,'Max')-get(h.sliders.CurTime,'Min'); 

+    set(h.sliders.CurTime,'SliderStep',[1.0/temp 5.0/temp]);  %Set Stepsize to constant [in seconds]

+

+    %update edit fields

+    set(h.edits.CurTime,'String',get(h.sliders.CurTime,'Value'));

+    set(h.edits.MinTime,'String',get(h.sliders.MinTime,'Value'));

+    set(h.edits.MaxTime,'String',get(h.sliders.MaxTime,'Value'));

+

+    %Finally, we have to redraw. Update time indices first.

+    [imintime,imaxtime]=FindMinMaxTimeIndices();

+    DrawRawData();                  %Rawdata only

+    DrawCurrentAircraftState();     %path info & markers

+end

+

+

+%% ************************************************************************

+%  CURTIME CALLBACK (nested function)

+%  ************************************************************************

+function curtime_callback(hObj,event) %#ok<INUSL>

+    %find current time

+    if(hObj==h.sliders.CurTime)

+        CurTime=get(h.sliders.CurTime,'Value');

+    elseif (hObj==h.edits.CurTime)

+        temp=str2num(get(h.edits.CurTime,'String'));

+        if(temp<maxtime && temp>mintime)

+            CurTime=temp;

+        else

+            %Error

+            set(h.labels.GUIState,'String','Error: You tried to set an invalid current time! Previous value restored.','BackgroundColor','red');

+        end

+    else

+        %Error

+        set(h.labels.GUIState,'String','Error: curtime_callback','BackgroundColor','red');        

+    end

+    

+    set(h.sliders.CurTime,'Value',CurTime);

+    set(h.edits.CurTime,'String',num2str(CurTime));

+    

+    %Redraw time markers, but don't have to redraw the whole raw data

+    DrawCurrentAircraftState();

+end

+

+%% ************************************************************************

+%  FINDMINMAXINDICES (nested function)

+%  ************************************************************************

+function [idxmin,idxmax] = FindMinMaxTimeIndices()

+    for i=1:size(sysvector.timestamp,1)

+        if time(i)>=mintime; idxmin=i; break; end

+    end

+    for i=1:size(sysvector.timestamp,1)

+        if maxtime==0; idxmax=size(sysvector.timestamp,1); break; end

+        if time(i)>=maxtime; idxmax=i; break; end

+    end

+    mintime=time(idxmin); 

+    maxtime=time(idxmax);

+end

+

+%% ************************************************************************

+%  ISVALIDHANDLE (nested function)

+%  ************************************************************************

+function isvalid = isvalidhandle(handle)

+   if(exist(varname(handle))>0 && length(ishandle(handle))>0) 

+        if(ishandle(handle)>0)

+            if(handle>0.0)

+                isvalid=true;

+                return;

+            end

+        end

+   end

+   isvalid=false;

+end

+

+%% ************************************************************************

+%  JUST SOME SMALL HELPER FUNCTIONS (nested function)

+%  ************************************************************************

+function out = varname(var)

+   out = inputname(1);

+end

+

+%This is the end of the matlab file / the main function

+end

diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp
index 74cbc5aaf..0d4894b8d 100644
--- a/apps/drivers/gps/ubx.cpp
+++ b/apps/drivers/gps/ubx.cpp
@@ -113,13 +113,15 @@ UBX::configure(unsigned &baudrate)
 		cfg_prt_packet.outProtoMask	= UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
 
 		send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
+		
+		/* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */
+		receive(UBX_CONFIG_TIMEOUT);
+		
 		if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) {
 			set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE);
 			baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE;
 		}
 
-		/* no ack is ecpected here, keep going configuring */
-
 		/* send a CFT-RATE message to define update rate */
 		type_gps_bin_cfg_rate_packet_t cfg_rate_packet;
 		memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet));
diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c
index 798e509e0..a30f0bf3c 100644
--- a/apps/mavlink/mavlink_receiver.c
+++ b/apps/mavlink/mavlink_receiver.c
@@ -395,7 +395,7 @@ handle_message(mavlink_message_t *msg)
 			static uint64_t old_timestamp = 0;
 
 			/* sensors general */
-			hil_sensors.timestamp = imu.time_usec;
+			hil_sensors.timestamp = hrt_absolute_time();
 
 			/* hil gyro */
 			static const float mrad2rad = 1.0e-3f;
@@ -446,12 +446,16 @@ handle_message(mavlink_message_t *msg)
 			hil_sensors.baro_alt_meter = h;
 			hil_sensors.baro_temp_celcius = imu.temperature;
 
+			hil_sensors.gyro_counter = hil_counter;
+			hil_sensors.magnetometer_counter = hil_counter;
+			hil_sensors.accelerometer_counter = hil_counter;
+
 			/* publish */
 			orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
 
 			// increment counters
-			hil_counter += 1 ;
-			hil_frames += 1 ;
+			hil_counter++;
+			hil_frames++;
 
 			// output
 			if ((timestamp - old_timestamp) > 10000000) {
@@ -481,13 +485,17 @@ handle_message(mavlink_message_t *msg)
 //			hil_gps.counter_pos_valid = hil_counter++;
 			hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m
 			hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m
-			hil_gps.s_variance_m_s = 100; // XXX 100 m/s variance?
-			hil_gps.p_variance_m = 100; // XXX 100 m variance?
+			hil_gps.s_variance_m_s = 5.0f;
+			hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m;
 			hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
-			hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(gps.cog * M_DEG_TO_RAD_F * 1e-2f);
-			hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(gps.cog * M_DEG_TO_RAD_F * 1e-2f);
+
+			/* gps.cog is in degrees 0..360 * 100, heading is -PI..PI */
+			float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f - M_PI_F;
+			hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(heading_rad);
+			hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(heading_rad);
 			hil_gps.vel_d_m_s = 0.0f;
-			hil_gps.cog_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; // from deg*100 to rad
+			/* COG (course over ground) is speced as 0..360 degrees (compass) */
+			hil_gps.cog_rad = heading_rad + M_PI_F; // from deg*100 to rad
 			hil_gps.fix_type = gps.fix_type;
 			hil_gps.satellites_visible = gps.satellites_visible;
 
diff --git a/apps/position_estimator_mc/Makefile b/apps/position_estimator_mc/Makefile
new file mode 100644
index 000000000..8f1100158
--- /dev/null
+++ b/apps/position_estimator_mc/Makefile
@@ -0,0 +1,64 @@
+############################################################################
+#
+#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Makefile to build the position estimator
+#
+
+APPNAME		 = position_estimator_mc
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 4096
+
+CSRCS		 = position_estimator_mc_main.c \
+			   position_estimator_mc_params.c \
+			   codegen/positionKalmanFilter1D_initialize.c \
+			   codegen/positionKalmanFilter1D_terminate.c \
+			   codegen/positionKalmanFilter1D.c \
+			   codegen/rt_nonfinite.c \
+			   codegen/rtGetInf.c \
+			   codegen/rtGetNaN.c \
+			   codegen/positionKalmanFilter1D_dT_initialize.c \
+			   codegen/positionKalmanFilter1D_dT_terminate.c \
+			   codegen/kalman_dlqe1.c \
+			   codegen/kalman_dlqe1_initialize.c \
+			   codegen/kalman_dlqe1_terminate.c \
+			   codegen/kalman_dlqe2.c \
+			   codegen/kalman_dlqe2_initialize.c \
+			   codegen/kalman_dlqe2_terminate.c \
+			   codegen/kalman_dlqe3.c \
+			   codegen/kalman_dlqe3_initialize.c \
+			   codegen/kalman_dlqe3_terminate.c \
+			   codegen/kalman_dlqe3_data.c \
+			   codegen/randn.c
+			   
+include $(APPDIR)/mk/app.mk
diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe1.c b/apps/position_estimator_mc/codegen/kalman_dlqe1.c
new file mode 100755
index 000000000..977565b8e
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/kalman_dlqe1.c
@@ -0,0 +1,58 @@
+/*
+ * kalman_dlqe1.c
+ *
+ * Code generation for function 'kalman_dlqe1'
+ *
+ * C source code generated on: Wed Feb 13 20:34:32 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "kalman_dlqe1.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void kalman_dlqe1(const real32_T A[9], const real32_T C[3], const real32_T K[3],
+                  const real32_T x_aposteriori_k[3], real32_T z, real32_T
+                  x_aposteriori[3])
+{
+	printf("[dlqe input]: x_aposteriori_k %12.8f\t %12.8f\t %12.8f\t z:%12.8f\n", (double)(x_aposteriori_k[0]), (double)(x_aposteriori_k[1]), (double)(x_aposteriori_k[2]), (double)z);
+	printf("[dlqe input]: C[0]: %12.8f\tC[1] %12.8f\tC[2] %12.8f\n", (double)(C[0]), (double)(C[1]), (double)(C[2]));
+  real32_T y;
+  int32_T i0;
+  real32_T b_y[3];
+  int32_T i1;
+  real32_T f0;
+  y = 0.0F;
+  for (i0 = 0; i0 < 3; i0++) {
+    b_y[i0] = 0.0F;
+    for (i1 = 0; i1 < 3; i1++) {
+      b_y[i0] += C[i1] * A[i1 + 3 * i0];
+    }
+
+    y += b_y[i0] * x_aposteriori_k[i0];
+  }
+
+  y = z - y;
+  for (i0 = 0; i0 < 3; i0++) {
+    f0 = 0.0F;
+    for (i1 = 0; i1 < 3; i1++) {
+      f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1];
+    }
+
+    x_aposteriori[i0] = f0 + K[i0] * y;
+  }
+  //printf("[dlqe output]: x_aposteriori %12.8f\t %12.8f\t %12.8f\n", (double)(x_aposteriori[0]), (double)(x_aposteriori[1]), (double)(x_aposteriori[2]));
+}
+
+/* End of code generation (kalman_dlqe1.c) */
diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe1.h b/apps/position_estimator_mc/codegen/kalman_dlqe1.h
new file mode 100755
index 000000000..2f5330563
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/kalman_dlqe1.h
@@ -0,0 +1,30 @@
+/*
+ * kalman_dlqe1.h
+ *
+ * Code generation for function 'kalman_dlqe1'
+ *
+ * C source code generated on: Wed Feb 13 20:34:32 2013
+ *
+ */
+
+#ifndef __KALMAN_DLQE1_H__
+#define __KALMAN_DLQE1_H__
+/* Include files */
+#include <stddef.h>
+#include <stdlib.h>
+
+#include "rtwtypes.h"
+#include "kalman_dlqe1_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void kalman_dlqe1(const real32_T A[9], const real32_T C[3], const real32_T K[3], const real32_T x_aposteriori_k[3], real32_T z, real32_T x_aposteriori[3]);
+#endif
+/* End of code generation (kalman_dlqe1.h) */
diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe1_initialize.c b/apps/position_estimator_mc/codegen/kalman_dlqe1_initialize.c
new file mode 100755
index 000000000..6627f76e9
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/kalman_dlqe1_initialize.c
@@ -0,0 +1,31 @@
+/*
+ * kalman_dlqe1_initialize.c
+ *
+ * Code generation for function 'kalman_dlqe1_initialize'
+ *
+ * C source code generated on: Wed Feb 13 20:34:31 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "kalman_dlqe1.h"
+#include "kalman_dlqe1_initialize.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void kalman_dlqe1_initialize(void)
+{
+  rt_InitInfAndNaN(8U);
+}
+
+/* End of code generation (kalman_dlqe1_initialize.c) */
diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe1_initialize.h b/apps/position_estimator_mc/codegen/kalman_dlqe1_initialize.h
new file mode 100755
index 000000000..a77eb5712
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/kalman_dlqe1_initialize.h
@@ -0,0 +1,30 @@
+/*
+ * kalman_dlqe1_initialize.h
+ *
+ * Code generation for function 'kalman_dlqe1_initialize'
+ *
+ * C source code generated on: Wed Feb 13 20:34:31 2013
+ *
+ */
+
+#ifndef __KALMAN_DLQE1_INITIALIZE_H__
+#define __KALMAN_DLQE1_INITIALIZE_H__
+/* Include files */
+#include <stddef.h>
+#include <stdlib.h>
+
+#include "rtwtypes.h"
+#include "kalman_dlqe1_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void kalman_dlqe1_initialize(void);
+#endif
+/* End of code generation (kalman_dlqe1_initialize.h) */
diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe1_terminate.c b/apps/position_estimator_mc/codegen/kalman_dlqe1_terminate.c
new file mode 100755
index 000000000..a65536f79
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/kalman_dlqe1_terminate.c
@@ -0,0 +1,31 @@
+/*
+ * kalman_dlqe1_terminate.c
+ *
+ * Code generation for function 'kalman_dlqe1_terminate'
+ *
+ * C source code generated on: Wed Feb 13 20:34:31 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "kalman_dlqe1.h"
+#include "kalman_dlqe1_terminate.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void kalman_dlqe1_terminate(void)
+{
+  /* (no terminate code required) */
+}
+
+/* End of code generation (kalman_dlqe1_terminate.c) */
diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe1_terminate.h b/apps/position_estimator_mc/codegen/kalman_dlqe1_terminate.h
new file mode 100755
index 000000000..100c7f76c
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/kalman_dlqe1_terminate.h
@@ -0,0 +1,30 @@
+/*
+ * kalman_dlqe1_terminate.h
+ *
+ * Code generation for function 'kalman_dlqe1_terminate'
+ *
+ * C source code generated on: Wed Feb 13 20:34:32 2013
+ *
+ */
+
+#ifndef __KALMAN_DLQE1_TERMINATE_H__
+#define __KALMAN_DLQE1_TERMINATE_H__
+/* Include files */
+#include <stddef.h>
+#include <stdlib.h>
+
+#include "rtwtypes.h"
+#include "kalman_dlqe1_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void kalman_dlqe1_terminate(void);
+#endif
+/* End of code generation (kalman_dlqe1_terminate.h) */
diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe1_types.h b/apps/position_estimator_mc/codegen/kalman_dlqe1_types.h
new file mode 100755
index 000000000..d4b2c2d61
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/kalman_dlqe1_types.h
@@ -0,0 +1,16 @@
+/*
+ * kalman_dlqe1_types.h
+ *
+ * Code generation for function 'kalman_dlqe1'
+ *
+ * C source code generated on: Wed Feb 13 20:34:31 2013
+ *
+ */
+
+#ifndef __KALMAN_DLQE1_TYPES_H__
+#define __KALMAN_DLQE1_TYPES_H__
+
+/* Type Definitions */
+
+#endif
+/* End of code generation (kalman_dlqe1_types.h) */
diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe2.c b/apps/position_estimator_mc/codegen/kalman_dlqe2.c
new file mode 100755
index 000000000..11b999064
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/kalman_dlqe2.c
@@ -0,0 +1,119 @@
+/*
+ * kalman_dlqe2.c
+ *
+ * Code generation for function 'kalman_dlqe2'
+ *
+ * C source code generated on: Thu Feb 14 12:52:28 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "kalman_dlqe2.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+static real32_T rt_powf_snf(real32_T u0, real32_T u1);
+
+/* Function Definitions */
+static real32_T rt_powf_snf(real32_T u0, real32_T u1)
+{
+  real32_T y;
+  real32_T f1;
+  real32_T f2;
+  if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
+    y = ((real32_T)rtNaN);
+  } else {
+    f1 = (real32_T)fabs(u0);
+    f2 = (real32_T)fabs(u1);
+    if (rtIsInfF(u1)) {
+      if (f1 == 1.0F) {
+        y = ((real32_T)rtNaN);
+      } else if (f1 > 1.0F) {
+        if (u1 > 0.0F) {
+          y = ((real32_T)rtInf);
+        } else {
+          y = 0.0F;
+        }
+      } else if (u1 > 0.0F) {
+        y = 0.0F;
+      } else {
+        y = ((real32_T)rtInf);
+      }
+    } else if (f2 == 0.0F) {
+      y = 1.0F;
+    } else if (f2 == 1.0F) {
+      if (u1 > 0.0F) {
+        y = u0;
+      } else {
+        y = 1.0F / u0;
+      }
+    } else if (u1 == 2.0F) {
+      y = u0 * u0;
+    } else if ((u1 == 0.5F) && (u0 >= 0.0F)) {
+      y = (real32_T)sqrt(u0);
+    } else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) {
+      y = ((real32_T)rtNaN);
+    } else {
+      y = (real32_T)pow(u0, u1);
+    }
+  }
+
+  return y;
+}
+
+void kalman_dlqe2(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const
+                  real32_T x_aposteriori_k[3], real32_T z, real32_T
+                  x_aposteriori[3])
+{
+	//printf("[dqle2] dt: %12.8f\tvk1 %12.8f\tk2: %12.8f\tk3: %12.8f\n", (double)(dt), (double)(k1), (double)(k2), (double)(k3));
+	//printf("[dqle2] dt: %8.4f\n", (double)(dt));//, (double)(k1), (double)(k2), (double)(k3));
+  real32_T A[9];
+  real32_T y;
+  int32_T i0;
+  static const int8_T iv0[3] = { 0, 0, 1 };
+
+  real32_T b_k1[3];
+  int32_T i1;
+  static const int8_T iv1[3] = { 1, 0, 0 };
+
+  real32_T f0;
+  A[0] = 1.0F;
+  A[3] = dt;
+  A[6] = 0.5F * rt_powf_snf(dt, 2.0F);
+  A[1] = 0.0F;
+  A[4] = 1.0F;
+  A[7] = dt;
+  y = 0.0F;
+  for (i0 = 0; i0 < 3; i0++) {
+    A[2 + 3 * i0] = (real32_T)iv0[i0];
+    b_k1[i0] = 0.0F;
+    for (i1 = 0; i1 < 3; i1++) {
+      b_k1[i0] += (real32_T)iv1[i1] * A[i1 + 3 * i0];
+    }
+
+    y += b_k1[i0] * x_aposteriori_k[i0];
+  }
+
+  y = z - y;
+  b_k1[0] = k1;
+  b_k1[1] = k2;
+  b_k1[2] = k3;
+  for (i0 = 0; i0 < 3; i0++) {
+    f0 = 0.0F;
+    for (i1 = 0; i1 < 3; i1++) {
+      f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1];
+    }
+
+    x_aposteriori[i0] = f0 + b_k1[i0] * y;
+  }
+}
+
+/* End of code generation (kalman_dlqe2.c) */
diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe2.h b/apps/position_estimator_mc/codegen/kalman_dlqe2.h
new file mode 100755
index 000000000..30170ae22
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/kalman_dlqe2.h
@@ -0,0 +1,32 @@
+/*
+ * kalman_dlqe2.h
+ *
+ * Code generation for function 'kalman_dlqe2'
+ *
+ * C source code generated on: Thu Feb 14 12:52:29 2013
+ *
+ */
+
+#ifndef __KALMAN_DLQE2_H__
+#define __KALMAN_DLQE2_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include "rt_nonfinite.h"
+
+#include "rtwtypes.h"
+#include "kalman_dlqe2_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void kalman_dlqe2(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const real32_T x_aposteriori_k[3], real32_T z, real32_T x_aposteriori[3]);
+#endif
+/* End of code generation (kalman_dlqe2.h) */
diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe2_initialize.c b/apps/position_estimator_mc/codegen/kalman_dlqe2_initialize.c
new file mode 100755
index 000000000..de5a1d8aa
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/kalman_dlqe2_initialize.c
@@ -0,0 +1,31 @@
+/*
+ * kalman_dlqe2_initialize.c
+ *
+ * Code generation for function 'kalman_dlqe2_initialize'
+ *
+ * C source code generated on: Thu Feb 14 12:52:28 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "kalman_dlqe2.h"
+#include "kalman_dlqe2_initialize.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void kalman_dlqe2_initialize(void)
+{
+  rt_InitInfAndNaN(8U);
+}
+
+/* End of code generation (kalman_dlqe2_initialize.c) */
diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe2_initialize.h b/apps/position_estimator_mc/codegen/kalman_dlqe2_initialize.h
new file mode 100755
index 000000000..3d507ff31
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/kalman_dlqe2_initialize.h
@@ -0,0 +1,32 @@
+/*
+ * kalman_dlqe2_initialize.h
+ *
+ * Code generation for function 'kalman_dlqe2_initialize'
+ *
+ * C source code generated on: Thu Feb 14 12:52:28 2013
+ *
+ */
+
+#ifndef __KALMAN_DLQE2_INITIALIZE_H__
+#define __KALMAN_DLQE2_INITIALIZE_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include "rt_nonfinite.h"
+
+#include "rtwtypes.h"
+#include "kalman_dlqe2_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void kalman_dlqe2_initialize(void);
+#endif
+/* End of code generation (kalman_dlqe2_initialize.h) */
diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe2_terminate.c b/apps/position_estimator_mc/codegen/kalman_dlqe2_terminate.c
new file mode 100755
index 000000000..0757c878c
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/kalman_dlqe2_terminate.c
@@ -0,0 +1,31 @@
+/*
+ * kalman_dlqe2_terminate.c
+ *
+ * Code generation for function 'kalman_dlqe2_terminate'
+ *
+ * C source code generated on: Thu Feb 14 12:52:28 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "kalman_dlqe2.h"
+#include "kalman_dlqe2_terminate.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void kalman_dlqe2_terminate(void)
+{
+  /* (no terminate code required) */
+}
+
+/* End of code generation (kalman_dlqe2_terminate.c) */
diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe2_terminate.h b/apps/position_estimator_mc/codegen/kalman_dlqe2_terminate.h
new file mode 100755
index 000000000..23995020b
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/kalman_dlqe2_terminate.h
@@ -0,0 +1,32 @@
+/*
+ * kalman_dlqe2_terminate.h
+ *
+ * Code generation for function 'kalman_dlqe2_terminate'
+ *
+ * C source code generated on: Thu Feb 14 12:52:28 2013
+ *
+ */
+
+#ifndef __KALMAN_DLQE2_TERMINATE_H__
+#define __KALMAN_DLQE2_TERMINATE_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include "rt_nonfinite.h"
+
+#include "rtwtypes.h"
+#include "kalman_dlqe2_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void kalman_dlqe2_terminate(void);
+#endif
+/* End of code generation (kalman_dlqe2_terminate.h) */
diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe2_types.h b/apps/position_estimator_mc/codegen/kalman_dlqe2_types.h
new file mode 100755
index 000000000..f7a04d908
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/kalman_dlqe2_types.h
@@ -0,0 +1,16 @@
+/*
+ * kalman_dlqe2_types.h
+ *
+ * Code generation for function 'kalman_dlqe2'
+ *
+ * C source code generated on: Thu Feb 14 12:52:28 2013
+ *
+ */
+
+#ifndef __KALMAN_DLQE2_TYPES_H__
+#define __KALMAN_DLQE2_TYPES_H__
+
+/* Type Definitions */
+
+#endif
+/* End of code generation (kalman_dlqe2_types.h) */
diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe3.c b/apps/position_estimator_mc/codegen/kalman_dlqe3.c
new file mode 100755
index 000000000..9efe2ea7a
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/kalman_dlqe3.c
@@ -0,0 +1,137 @@
+/*

+ * kalman_dlqe3.c

+ *

+ * Code generation for function 'kalman_dlqe3'

+ *

+ * C source code generated on: Tue Feb 19 15:26:31 2013

+ *

+ */

+

+/* Include files */

+#include "rt_nonfinite.h"

+#include "kalman_dlqe3.h"

+#include "randn.h"

+

+/* Type Definitions */

+

+/* Named Constants */

+

+/* Variable Declarations */

+

+/* Variable Definitions */

+

+/* Function Declarations */

+static real32_T rt_powf_snf(real32_T u0, real32_T u1);

+

+/* Function Definitions */

+static real32_T rt_powf_snf(real32_T u0, real32_T u1)

+{

+  real32_T y;

+  real32_T f1;

+  real32_T f2;

+  if (rtIsNaNF(u0) || rtIsNaNF(u1)) {

+    y = ((real32_T)rtNaN);

+  } else {

+    f1 = (real32_T)fabs(u0);

+    f2 = (real32_T)fabs(u1);

+    if (rtIsInfF(u1)) {

+      if (f1 == 1.0F) {

+        y = ((real32_T)rtNaN);

+      } else if (f1 > 1.0F) {

+        if (u1 > 0.0F) {

+          y = ((real32_T)rtInf);

+        } else {

+          y = 0.0F;

+        }

+      } else if (u1 > 0.0F) {

+        y = 0.0F;

+      } else {

+        y = ((real32_T)rtInf);

+      }

+    } else if (f2 == 0.0F) {

+      y = 1.0F;

+    } else if (f2 == 1.0F) {

+      if (u1 > 0.0F) {

+        y = u0;

+      } else {

+        y = 1.0F / u0;

+      }

+    } else if (u1 == 2.0F) {

+      y = u0 * u0;

+    } else if ((u1 == 0.5F) && (u0 >= 0.0F)) {

+      y = (real32_T)sqrt(u0);

+    } else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) {

+      y = ((real32_T)rtNaN);

+    } else {

+      y = (real32_T)pow(u0, u1);

+    }

+  }

+

+  return y;

+}

+

+void kalman_dlqe3(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const

+                  real32_T x_aposteriori_k[3], real32_T z, real32_T posUpdate,

+                  real32_T addNoise, real32_T sigma, real32_T x_aposteriori[3])

+{

+  real32_T A[9];

+  int32_T i0;

+  static const int8_T iv0[3] = { 0, 0, 1 };

+

+  real_T b;

+  real32_T y;

+  real32_T b_y[3];

+  int32_T i1;

+  static const int8_T iv1[3] = { 1, 0, 0 };

+

+  real32_T b_k1[3];

+  real32_T f0;

+  A[0] = 1.0F;

+  A[3] = dt;

+  A[6] = 0.5F * rt_powf_snf(dt, 2.0F);

+  A[1] = 0.0F;

+  A[4] = 1.0F;

+  A[7] = dt;

+  for (i0 = 0; i0 < 3; i0++) {

+    A[2 + 3 * i0] = (real32_T)iv0[i0];

+  }

+

+  if (addNoise == 1.0F) {

+    b = randn();

+    z += sigma * (real32_T)b;

+  }

+

+  if (posUpdate != 0.0F) {

+    y = 0.0F;

+    for (i0 = 0; i0 < 3; i0++) {

+      b_y[i0] = 0.0F;

+      for (i1 = 0; i1 < 3; i1++) {

+        b_y[i0] += (real32_T)iv1[i1] * A[i1 + 3 * i0];

+      }

+

+      y += b_y[i0] * x_aposteriori_k[i0];

+    }

+

+    y = z - y;

+    b_k1[0] = k1;

+    b_k1[1] = k2;

+    b_k1[2] = k3;

+    for (i0 = 0; i0 < 3; i0++) {

+      f0 = 0.0F;

+      for (i1 = 0; i1 < 3; i1++) {

+        f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1];

+      }

+

+      x_aposteriori[i0] = f0 + b_k1[i0] * y;

+    }

+  } else {

+    for (i0 = 0; i0 < 3; i0++) {

+      x_aposteriori[i0] = 0.0F;

+      for (i1 = 0; i1 < 3; i1++) {

+        x_aposteriori[i0] += A[i0 + 3 * i1] * x_aposteriori_k[i1];

+      }

+    }

+  }

+}

+

+/* End of code generation (kalman_dlqe3.c) */

diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe3.h b/apps/position_estimator_mc/codegen/kalman_dlqe3.h
new file mode 100755
index 000000000..9bbffbbb3
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/kalman_dlqe3.h
@@ -0,0 +1,33 @@
+/*

+ * kalman_dlqe3.h

+ *

+ * Code generation for function 'kalman_dlqe3'

+ *

+ * C source code generated on: Tue Feb 19 15:26:32 2013

+ *

+ */

+

+#ifndef __KALMAN_DLQE3_H__

+#define __KALMAN_DLQE3_H__

+/* Include files */

+#include <math.h>

+#include <stddef.h>

+#include <stdlib.h>

+#include <string.h>

+#include "rt_nonfinite.h"

+

+#include "rtwtypes.h"

+#include "kalman_dlqe3_types.h"

+

+/* Type Definitions */

+

+/* Named Constants */

+

+/* Variable Declarations */

+

+/* Variable Definitions */

+

+/* Function Declarations */

+extern void kalman_dlqe3(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const real32_T x_aposteriori_k[3], real32_T z, real32_T posUpdate, real32_T addNoise, real32_T sigma, real32_T x_aposteriori[3]);

+#endif

+/* End of code generation (kalman_dlqe3.h) */

diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe3_data.c b/apps/position_estimator_mc/codegen/kalman_dlqe3_data.c
new file mode 100755
index 000000000..8f2275c13
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/kalman_dlqe3_data.c
@@ -0,0 +1,32 @@
+/*

+ * kalman_dlqe3_data.c

+ *

+ * Code generation for function 'kalman_dlqe3_data'

+ *

+ * C source code generated on: Tue Feb 19 15:26:31 2013

+ *

+ */

+

+/* Include files */

+#include "rt_nonfinite.h"

+#include "kalman_dlqe3.h"

+#include "kalman_dlqe3_data.h"

+

+/* Type Definitions */

+

+/* Named Constants */

+

+/* Variable Declarations */

+

+/* Variable Definitions */

+uint32_T method;

+uint32_T state[2];

+uint32_T b_method;

+uint32_T b_state;

+uint32_T c_state[2];

+boolean_T state_not_empty;

+

+/* Function Declarations */

+

+/* Function Definitions */

+/* End of code generation (kalman_dlqe3_data.c) */

diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe3_data.h b/apps/position_estimator_mc/codegen/kalman_dlqe3_data.h
new file mode 100755
index 000000000..952eb7b89
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/kalman_dlqe3_data.h
@@ -0,0 +1,38 @@
+/*

+ * kalman_dlqe3_data.h

+ *

+ * Code generation for function 'kalman_dlqe3_data'

+ *

+ * C source code generated on: Tue Feb 19 15:26:31 2013

+ *

+ */

+

+#ifndef __KALMAN_DLQE3_DATA_H__

+#define __KALMAN_DLQE3_DATA_H__

+/* Include files */

+#include <math.h>

+#include <stddef.h>

+#include <stdlib.h>

+#include <string.h>

+#include "rt_nonfinite.h"

+

+#include "rtwtypes.h"

+#include "kalman_dlqe3_types.h"

+

+/* Type Definitions */

+

+/* Named Constants */

+

+/* Variable Declarations */

+extern uint32_T method;

+extern uint32_T state[2];

+extern uint32_T b_method;

+extern uint32_T b_state;

+extern uint32_T c_state[2];

+extern boolean_T state_not_empty;

+

+/* Variable Definitions */

+

+/* Function Declarations */

+#endif

+/* End of code generation (kalman_dlqe3_data.h) */

diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe3_initialize.c b/apps/position_estimator_mc/codegen/kalman_dlqe3_initialize.c
new file mode 100755
index 000000000..b87d604c4
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/kalman_dlqe3_initialize.c
@@ -0,0 +1,47 @@
+/*

+ * kalman_dlqe3_initialize.c

+ *

+ * Code generation for function 'kalman_dlqe3_initialize'

+ *

+ * C source code generated on: Tue Feb 19 15:26:31 2013

+ *

+ */

+

+/* Include files */

+#include "rt_nonfinite.h"

+#include "kalman_dlqe3.h"

+#include "kalman_dlqe3_initialize.h"

+#include "kalman_dlqe3_data.h"

+

+/* Type Definitions */

+

+/* Named Constants */

+

+/* Variable Declarations */

+

+/* Variable Definitions */

+

+/* Function Declarations */

+

+/* Function Definitions */

+void kalman_dlqe3_initialize(void)

+{

+  int32_T i;

+  static const uint32_T uv0[2] = { 362436069U, 0U };

+

+  rt_InitInfAndNaN(8U);

+  state_not_empty = FALSE;

+  b_state = 1144108930U;

+  b_method = 7U;

+  method = 0U;

+  for (i = 0; i < 2; i++) {

+    c_state[i] = 362436069U + 158852560U * (uint32_T)i;

+    state[i] = uv0[i];

+  }

+

+  if (state[1] == 0U) {

+    state[1] = 521288629U;

+  }

+}

+

+/* End of code generation (kalman_dlqe3_initialize.c) */

diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe3_initialize.h b/apps/position_estimator_mc/codegen/kalman_dlqe3_initialize.h
new file mode 100755
index 000000000..9dee90f9e
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/kalman_dlqe3_initialize.h
@@ -0,0 +1,33 @@
+/*

+ * kalman_dlqe3_initialize.h

+ *

+ * Code generation for function 'kalman_dlqe3_initialize'

+ *

+ * C source code generated on: Tue Feb 19 15:26:31 2013

+ *

+ */

+

+#ifndef __KALMAN_DLQE3_INITIALIZE_H__

+#define __KALMAN_DLQE3_INITIALIZE_H__

+/* Include files */

+#include <math.h>

+#include <stddef.h>

+#include <stdlib.h>

+#include <string.h>

+#include "rt_nonfinite.h"

+

+#include "rtwtypes.h"

+#include "kalman_dlqe3_types.h"

+

+/* Type Definitions */

+

+/* Named Constants */

+

+/* Variable Declarations */

+

+/* Variable Definitions */

+

+/* Function Declarations */

+extern void kalman_dlqe3_initialize(void);

+#endif

+/* End of code generation (kalman_dlqe3_initialize.h) */

diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe3_terminate.c b/apps/position_estimator_mc/codegen/kalman_dlqe3_terminate.c
new file mode 100755
index 000000000..b00858205
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/kalman_dlqe3_terminate.c
@@ -0,0 +1,31 @@
+/*

+ * kalman_dlqe3_terminate.c

+ *

+ * Code generation for function 'kalman_dlqe3_terminate'

+ *

+ * C source code generated on: Tue Feb 19 15:26:31 2013

+ *

+ */

+

+/* Include files */

+#include "rt_nonfinite.h"

+#include "kalman_dlqe3.h"

+#include "kalman_dlqe3_terminate.h"

+

+/* Type Definitions */

+

+/* Named Constants */

+

+/* Variable Declarations */

+

+/* Variable Definitions */

+

+/* Function Declarations */

+

+/* Function Definitions */

+void kalman_dlqe3_terminate(void)

+{

+  /* (no terminate code required) */

+}

+

+/* End of code generation (kalman_dlqe3_terminate.c) */

diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe3_terminate.h b/apps/position_estimator_mc/codegen/kalman_dlqe3_terminate.h
new file mode 100755
index 000000000..69cc85c76
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/kalman_dlqe3_terminate.h
@@ -0,0 +1,33 @@
+/*

+ * kalman_dlqe3_terminate.h

+ *

+ * Code generation for function 'kalman_dlqe3_terminate'

+ *

+ * C source code generated on: Tue Feb 19 15:26:31 2013

+ *

+ */

+

+#ifndef __KALMAN_DLQE3_TERMINATE_H__

+#define __KALMAN_DLQE3_TERMINATE_H__

+/* Include files */

+#include <math.h>

+#include <stddef.h>

+#include <stdlib.h>

+#include <string.h>

+#include "rt_nonfinite.h"

+

+#include "rtwtypes.h"

+#include "kalman_dlqe3_types.h"

+

+/* Type Definitions */

+

+/* Named Constants */

+

+/* Variable Declarations */

+

+/* Variable Definitions */

+

+/* Function Declarations */

+extern void kalman_dlqe3_terminate(void);

+#endif

+/* End of code generation (kalman_dlqe3_terminate.h) */

diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe3_types.h b/apps/position_estimator_mc/codegen/kalman_dlqe3_types.h
new file mode 100755
index 000000000..f36ea4557
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/kalman_dlqe3_types.h
@@ -0,0 +1,16 @@
+/*

+ * kalman_dlqe3_types.h

+ *

+ * Code generation for function 'kalman_dlqe3'

+ *

+ * C source code generated on: Tue Feb 19 15:26:30 2013

+ *

+ */

+

+#ifndef __KALMAN_DLQE3_TYPES_H__

+#define __KALMAN_DLQE3_TYPES_H__

+

+/* Type Definitions */

+

+#endif

+/* End of code generation (kalman_dlqe3_types.h) */

diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D.c b/apps/position_estimator_mc/codegen/positionKalmanFilter1D.c
new file mode 100755
index 000000000..5139848bc
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D.c
@@ -0,0 +1,136 @@
+/*
+ * positionKalmanFilter1D.c
+ *
+ * Code generation for function 'positionKalmanFilter1D'
+ *
+ * C source code generated on: Fri Nov 30 14:26:11 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "positionKalmanFilter1D.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void positionKalmanFilter1D(const real32_T A[9], const real32_T B[3], const
+  real32_T C[3], const real32_T x_aposteriori_k[3], const real32_T
+  P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T
+  Q[9], real32_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3],
+  real32_T P_aposteriori[9])
+{
+  int32_T i0;
+  real32_T f0;
+  int32_T k;
+  real32_T b_A[9];
+  int32_T i1;
+  real32_T P_apriori[9];
+  real32_T y;
+  real32_T K[3];
+  real32_T S;
+  int8_T I[9];
+
+  /* prediction */
+  for (i0 = 0; i0 < 3; i0++) {
+    f0 = 0.0F;
+    for (k = 0; k < 3; k++) {
+      f0 += A[i0 + 3 * k] * x_aposteriori_k[k];
+    }
+
+    x_aposteriori[i0] = f0 + B[i0] * u;
+  }
+
+  for (i0 = 0; i0 < 3; i0++) {
+    for (k = 0; k < 3; k++) {
+      b_A[i0 + 3 * k] = 0.0F;
+      for (i1 = 0; i1 < 3; i1++) {
+        b_A[i0 + 3 * k] += A[i0 + 3 * i1] * P_aposteriori_k[i1 + 3 * k];
+      }
+    }
+  }
+
+  for (i0 = 0; i0 < 3; i0++) {
+    for (k = 0; k < 3; k++) {
+      f0 = 0.0F;
+      for (i1 = 0; i1 < 3; i1++) {
+        f0 += b_A[i0 + 3 * i1] * A[k + 3 * i1];
+      }
+
+      P_apriori[i0 + 3 * k] = f0 + Q[i0 + 3 * k];
+    }
+  }
+
+  if ((real32_T)fabs(u) < thresh) {
+    x_aposteriori[1] *= decay;
+  }
+
+  /* update */
+  if (gps_update == 1) {
+    y = 0.0F;
+    for (k = 0; k < 3; k++) {
+      y += C[k] * x_aposteriori[k];
+      K[k] = 0.0F;
+      for (i0 = 0; i0 < 3; i0++) {
+        K[k] += C[i0] * P_apriori[i0 + 3 * k];
+      }
+    }
+
+    y = z - y;
+    S = 0.0F;
+    for (k = 0; k < 3; k++) {
+      S += K[k] * C[k];
+    }
+
+    S += R;
+    for (i0 = 0; i0 < 3; i0++) {
+      f0 = 0.0F;
+      for (k = 0; k < 3; k++) {
+        f0 += P_apriori[i0 + 3 * k] * C[k];
+      }
+
+      K[i0] = f0 / S;
+    }
+
+    for (i0 = 0; i0 < 3; i0++) {
+      x_aposteriori[i0] += K[i0] * y;
+    }
+
+    for (i0 = 0; i0 < 9; i0++) {
+      I[i0] = 0;
+    }
+
+    for (k = 0; k < 3; k++) {
+      I[k + 3 * k] = 1;
+    }
+
+    for (i0 = 0; i0 < 3; i0++) {
+      for (k = 0; k < 3; k++) {
+        b_A[k + 3 * i0] = (real32_T)I[k + 3 * i0] - K[k] * C[i0];
+      }
+    }
+
+    for (i0 = 0; i0 < 3; i0++) {
+      for (k = 0; k < 3; k++) {
+        P_aposteriori[i0 + 3 * k] = 0.0F;
+        for (i1 = 0; i1 < 3; i1++) {
+          P_aposteriori[i0 + 3 * k] += b_A[i0 + 3 * i1] * P_apriori[i1 + 3 * k];
+        }
+      }
+    }
+  } else {
+    for (i0 = 0; i0 < 9; i0++) {
+      P_aposteriori[i0] = P_apriori[i0];
+    }
+  }
+}
+
+/* End of code generation (positionKalmanFilter1D.c) */
diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D.h b/apps/position_estimator_mc/codegen/positionKalmanFilter1D.h
new file mode 100755
index 000000000..205c8eb4e
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D.h
@@ -0,0 +1,31 @@
+/*
+ * positionKalmanFilter1D.h
+ *
+ * Code generation for function 'positionKalmanFilter1D'
+ *
+ * C source code generated on: Fri Nov 30 14:26:11 2012
+ *
+ */
+
+#ifndef __POSITIONKALMANFILTER1D_H__
+#define __POSITIONKALMANFILTER1D_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+
+#include "rtwtypes.h"
+#include "positionKalmanFilter1D_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void positionKalmanFilter1D(const real32_T A[9], const real32_T B[3], const real32_T C[3], const real32_T x_aposteriori_k[3], const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T Q[9], real32_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], real32_T P_aposteriori[9]);
+#endif
+/* End of code generation (positionKalmanFilter1D.h) */
diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c
new file mode 100755
index 000000000..4c535618a
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c
@@ -0,0 +1,157 @@
+/*
+ * positionKalmanFilter1D_dT.c
+ *
+ * Code generation for function 'positionKalmanFilter1D_dT'
+ *
+ * C source code generated on: Fri Nov 30 17:37:33 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "positionKalmanFilter1D_dT.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void positionKalmanFilter1D_dT(real32_T dT, const real32_T x_aposteriori_k[3],
+  const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update,
+  const real32_T Q[9], real_T R, real32_T thresh, real32_T decay, real32_T
+  x_aposteriori[3], real32_T P_aposteriori[9])
+{
+  real32_T A[9];
+  int32_T i;
+  static const int8_T iv0[3] = { 0, 0, 1 };
+
+  real32_T K[3];
+  real32_T f0;
+  int32_T i0;
+  real32_T b_A[9];
+  int32_T i1;
+  real32_T P_apriori[9];
+  static const int8_T iv1[3] = { 1, 0, 0 };
+
+  real32_T fv0[3];
+  real32_T y;
+  static const int8_T iv2[3] = { 1, 0, 0 };
+
+  real32_T S;
+  int8_T I[9];
+
+  /* dynamics */
+  A[0] = 1.0F;
+  A[3] = dT;
+  A[6] = -0.5F * dT * dT;
+  A[1] = 0.0F;
+  A[4] = 1.0F;
+  A[7] = -dT;
+  for (i = 0; i < 3; i++) {
+    A[2 + 3 * i] = (real32_T)iv0[i];
+  }
+
+  /* prediction */
+  K[0] = 0.5F * dT * dT;
+  K[1] = dT;
+  K[2] = 0.0F;
+  for (i = 0; i < 3; i++) {
+    f0 = 0.0F;
+    for (i0 = 0; i0 < 3; i0++) {
+      f0 += A[i + 3 * i0] * x_aposteriori_k[i0];
+    }
+
+    x_aposteriori[i] = f0 + K[i] * u;
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      b_A[i + 3 * i0] = 0.0F;
+      for (i1 = 0; i1 < 3; i1++) {
+        b_A[i + 3 * i0] += A[i + 3 * i1] * P_aposteriori_k[i1 + 3 * i0];
+      }
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      f0 = 0.0F;
+      for (i1 = 0; i1 < 3; i1++) {
+        f0 += b_A[i + 3 * i1] * A[i0 + 3 * i1];
+      }
+
+      P_apriori[i + 3 * i0] = f0 + Q[i + 3 * i0];
+    }
+  }
+
+  if ((real32_T)fabs(u) < thresh) {
+    x_aposteriori[1] *= decay;
+  }
+
+  /* update */
+  if (gps_update == 1) {
+    f0 = 0.0F;
+    for (i = 0; i < 3; i++) {
+      f0 += (real32_T)iv1[i] * x_aposteriori[i];
+      fv0[i] = 0.0F;
+      for (i0 = 0; i0 < 3; i0++) {
+        fv0[i] += (real32_T)iv1[i0] * P_apriori[i0 + 3 * i];
+      }
+    }
+
+    y = z - f0;
+    f0 = 0.0F;
+    for (i = 0; i < 3; i++) {
+      f0 += fv0[i] * (real32_T)iv2[i];
+    }
+
+    S = f0 + (real32_T)R;
+    for (i = 0; i < 3; i++) {
+      f0 = 0.0F;
+      for (i0 = 0; i0 < 3; i0++) {
+        f0 += P_apriori[i + 3 * i0] * (real32_T)iv2[i0];
+      }
+
+      K[i] = f0 / S;
+    }
+
+    for (i = 0; i < 3; i++) {
+      x_aposteriori[i] += K[i] * y;
+    }
+
+    for (i = 0; i < 9; i++) {
+      I[i] = 0;
+    }
+
+    for (i = 0; i < 3; i++) {
+      I[i + 3 * i] = 1;
+    }
+
+    for (i = 0; i < 3; i++) {
+      for (i0 = 0; i0 < 3; i0++) {
+        A[i0 + 3 * i] = (real32_T)I[i0 + 3 * i] - K[i0] * (real32_T)iv1[i];
+      }
+    }
+
+    for (i = 0; i < 3; i++) {
+      for (i0 = 0; i0 < 3; i0++) {
+        P_aposteriori[i + 3 * i0] = 0.0F;
+        for (i1 = 0; i1 < 3; i1++) {
+          P_aposteriori[i + 3 * i0] += A[i + 3 * i1] * P_apriori[i1 + 3 * i0];
+        }
+      }
+    }
+  } else {
+    for (i = 0; i < 9; i++) {
+      P_aposteriori[i] = P_apriori[i];
+    }
+  }
+}
+
+/* End of code generation (positionKalmanFilter1D_dT.c) */
diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h
new file mode 100755
index 000000000..94cbe2ce6
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h
@@ -0,0 +1,31 @@
+/*
+ * positionKalmanFilter1D_dT.h
+ *
+ * Code generation for function 'positionKalmanFilter1D_dT'
+ *
+ * C source code generated on: Fri Nov 30 17:37:33 2012
+ *
+ */
+
+#ifndef __POSITIONKALMANFILTER1D_DT_H__
+#define __POSITIONKALMANFILTER1D_DT_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+
+#include "rtwtypes.h"
+#include "positionKalmanFilter1D_dT_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void positionKalmanFilter1D_dT(real32_T dT, const real32_T x_aposteriori_k[3], const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T Q[9], real_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], real32_T P_aposteriori[9]);
+#endif
+/* End of code generation (positionKalmanFilter1D_dT.h) */
diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c
new file mode 100755
index 000000000..aa89f8a9d
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c
@@ -0,0 +1,31 @@
+/*
+ * positionKalmanFilter1D_dT_initialize.c
+ *
+ * Code generation for function 'positionKalmanFilter1D_dT_initialize'
+ *
+ * C source code generated on: Fri Nov 30 17:37:33 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "positionKalmanFilter1D_dT.h"
+#include "positionKalmanFilter1D_dT_initialize.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void positionKalmanFilter1D_dT_initialize(void)
+{
+  rt_InitInfAndNaN(8U);
+}
+
+/* End of code generation (positionKalmanFilter1D_dT_initialize.c) */
diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h
new file mode 100755
index 000000000..8d358a9a3
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h
@@ -0,0 +1,31 @@
+/*
+ * positionKalmanFilter1D_dT_initialize.h
+ *
+ * Code generation for function 'positionKalmanFilter1D_dT_initialize'
+ *
+ * C source code generated on: Fri Nov 30 17:37:33 2012
+ *
+ */
+
+#ifndef __POSITIONKALMANFILTER1D_DT_INITIALIZE_H__
+#define __POSITIONKALMANFILTER1D_DT_INITIALIZE_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+
+#include "rtwtypes.h"
+#include "positionKalmanFilter1D_dT_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void positionKalmanFilter1D_dT_initialize(void);
+#endif
+/* End of code generation (positionKalmanFilter1D_dT_initialize.h) */
diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c
new file mode 100755
index 000000000..20ed2edbb
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c
@@ -0,0 +1,31 @@
+/*
+ * positionKalmanFilter1D_dT_terminate.c
+ *
+ * Code generation for function 'positionKalmanFilter1D_dT_terminate'
+ *
+ * C source code generated on: Fri Nov 30 17:37:33 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "positionKalmanFilter1D_dT.h"
+#include "positionKalmanFilter1D_dT_terminate.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void positionKalmanFilter1D_dT_terminate(void)
+{
+  /* (no terminate code required) */
+}
+
+/* End of code generation (positionKalmanFilter1D_dT_terminate.c) */
diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h
new file mode 100755
index 000000000..5eb5807a0
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h
@@ -0,0 +1,31 @@
+/*
+ * positionKalmanFilter1D_dT_terminate.h
+ *
+ * Code generation for function 'positionKalmanFilter1D_dT_terminate'
+ *
+ * C source code generated on: Fri Nov 30 17:37:33 2012
+ *
+ */
+
+#ifndef __POSITIONKALMANFILTER1D_DT_TERMINATE_H__
+#define __POSITIONKALMANFILTER1D_DT_TERMINATE_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+
+#include "rtwtypes.h"
+#include "positionKalmanFilter1D_dT_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void positionKalmanFilter1D_dT_terminate(void);
+#endif
+/* End of code generation (positionKalmanFilter1D_dT_terminate.h) */
diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h
new file mode 100755
index 000000000..43e5f016c
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h
@@ -0,0 +1,16 @@
+/*
+ * positionKalmanFilter1D_dT_types.h
+ *
+ * Code generation for function 'positionKalmanFilter1D_dT'
+ *
+ * C source code generated on: Fri Nov 30 17:37:33 2012
+ *
+ */
+
+#ifndef __POSITIONKALMANFILTER1D_DT_TYPES_H__
+#define __POSITIONKALMANFILTER1D_DT_TYPES_H__
+
+/* Type Definitions */
+
+#endif
+/* End of code generation (positionKalmanFilter1D_dT_types.h) */
diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c
new file mode 100755
index 000000000..5bd87c390
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c
@@ -0,0 +1,31 @@
+/*
+ * positionKalmanFilter1D_initialize.c
+ *
+ * Code generation for function 'positionKalmanFilter1D_initialize'
+ *
+ * C source code generated on: Fri Nov 30 14:26:11 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "positionKalmanFilter1D.h"
+#include "positionKalmanFilter1D_initialize.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void positionKalmanFilter1D_initialize(void)
+{
+  rt_InitInfAndNaN(8U);
+}
+
+/* End of code generation (positionKalmanFilter1D_initialize.c) */
diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h
new file mode 100755
index 000000000..44bce472f
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h
@@ -0,0 +1,31 @@
+/*
+ * positionKalmanFilter1D_initialize.h
+ *
+ * Code generation for function 'positionKalmanFilter1D_initialize'
+ *
+ * C source code generated on: Fri Nov 30 14:26:11 2012
+ *
+ */
+
+#ifndef __POSITIONKALMANFILTER1D_INITIALIZE_H__
+#define __POSITIONKALMANFILTER1D_INITIALIZE_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+
+#include "rtwtypes.h"
+#include "positionKalmanFilter1D_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void positionKalmanFilter1D_initialize(void);
+#endif
+/* End of code generation (positionKalmanFilter1D_initialize.h) */
diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c
new file mode 100755
index 000000000..41e11936f
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c
@@ -0,0 +1,31 @@
+/*
+ * positionKalmanFilter1D_terminate.c
+ *
+ * Code generation for function 'positionKalmanFilter1D_terminate'
+ *
+ * C source code generated on: Fri Nov 30 14:26:11 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "positionKalmanFilter1D.h"
+#include "positionKalmanFilter1D_terminate.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void positionKalmanFilter1D_terminate(void)
+{
+  /* (no terminate code required) */
+}
+
+/* End of code generation (positionKalmanFilter1D_terminate.c) */
diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h
new file mode 100755
index 000000000..e84ea01bc
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h
@@ -0,0 +1,31 @@
+/*
+ * positionKalmanFilter1D_terminate.h
+ *
+ * Code generation for function 'positionKalmanFilter1D_terminate'
+ *
+ * C source code generated on: Fri Nov 30 14:26:11 2012
+ *
+ */
+
+#ifndef __POSITIONKALMANFILTER1D_TERMINATE_H__
+#define __POSITIONKALMANFILTER1D_TERMINATE_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+
+#include "rtwtypes.h"
+#include "positionKalmanFilter1D_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void positionKalmanFilter1D_terminate(void);
+#endif
+/* End of code generation (positionKalmanFilter1D_terminate.h) */
diff --git a/apps/position_estimator_mc/codegen/positionKalmanFilter1D_types.h b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_types.h
new file mode 100755
index 000000000..4b473f56f
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/positionKalmanFilter1D_types.h
@@ -0,0 +1,16 @@
+/*
+ * positionKalmanFilter1D_types.h
+ *
+ * Code generation for function 'positionKalmanFilter1D'
+ *
+ * C source code generated on: Fri Nov 30 14:26:11 2012
+ *
+ */
+
+#ifndef __POSITIONKALMANFILTER1D_TYPES_H__
+#define __POSITIONKALMANFILTER1D_TYPES_H__
+
+/* Type Definitions */
+
+#endif
+/* End of code generation (positionKalmanFilter1D_types.h) */
diff --git a/apps/position_estimator_mc/codegen/randn.c b/apps/position_estimator_mc/codegen/randn.c
new file mode 100755
index 000000000..51aef7b76
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/randn.c
@@ -0,0 +1,524 @@
+/*

+ * randn.c

+ *

+ * Code generation for function 'randn'

+ *

+ * C source code generated on: Tue Feb 19 15:26:32 2013

+ *

+ */

+

+/* Include files */

+#include "rt_nonfinite.h"

+#include "kalman_dlqe3.h"

+#include "randn.h"

+#include "kalman_dlqe3_data.h"

+

+/* Type Definitions */

+

+/* Named Constants */

+

+/* Variable Declarations */

+

+/* Variable Definitions */

+static uint32_T d_state[625];

+

+/* Function Declarations */

+static real_T b_genrandu(uint32_T mt[625]);

+static real_T eml_rand_mt19937ar(uint32_T e_state[625]);

+static real_T eml_rand_shr3cong(uint32_T e_state[2]);

+static void genrand_uint32_vector(uint32_T mt[625], uint32_T u[2]);

+static void genrandu(uint32_T s, uint32_T *e_state, real_T *r);

+static void twister_state_vector(uint32_T mt[625], real_T seed);

+

+/* Function Definitions */

+static real_T b_genrandu(uint32_T mt[625])

+{

+  real_T r;

+  int32_T exitg1;

+  uint32_T u[2];

+  boolean_T isvalid;

+  int32_T k;

+  boolean_T exitg2;

+

+  /* <LEGAL>   This is a uniform (0,1) pseudorandom number generator based on: */

+  /* <LEGAL> */

+  /* <LEGAL>    A C-program for MT19937, with initialization improved 2002/1/26. */

+  /* <LEGAL>    Coded by Takuji Nishimura and Makoto Matsumoto. */

+  /* <LEGAL> */

+  /* <LEGAL>    Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura, */

+  /* <LEGAL>    All rights reserved. */

+  /* <LEGAL> */

+  /* <LEGAL>    Redistribution and use in source and binary forms, with or without */

+  /* <LEGAL>    modification, are permitted provided that the following conditions */

+  /* <LEGAL>    are met: */

+  /* <LEGAL> */

+  /* <LEGAL>      1. Redistributions of source code must retain the above copyright */

+  /* <LEGAL>         notice, this list of conditions and the following disclaimer. */

+  /* <LEGAL> */

+  /* <LEGAL>      2. Redistributions in binary form must reproduce the above copyright */

+  /* <LEGAL>         notice, this list of conditions and the following disclaimer in the */

+  /* <LEGAL>         documentation and/or other materials provided with the distribution. */

+  /* <LEGAL> */

+  /* <LEGAL>      3. The names of its contributors may not be used to endorse or promote */

+  /* <LEGAL>         products derived from this software without specific prior written */

+  /* <LEGAL>         permission. */

+  /* <LEGAL> */

+  /* <LEGAL>    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS */

+  /* <LEGAL>    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT */

+  /* <LEGAL>    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR */

+  /* <LEGAL>    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR */

+  /* <LEGAL>    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, */

+  /* <LEGAL>    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, */

+  /* <LEGAL>    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR */

+  /* <LEGAL>    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF */

+  /* <LEGAL>    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING */

+  /* <LEGAL>    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS */

+  /* <LEGAL>    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */

+  do {

+    exitg1 = 0;

+    genrand_uint32_vector(mt, u);

+    r = 1.1102230246251565E-16 * ((real_T)(u[0] >> 5U) * 6.7108864E+7 + (real_T)

+                                  (u[1] >> 6U));

+    if (r == 0.0) {

+      if ((mt[624] >= 1U) && (mt[624] < 625U)) {

+        isvalid = TRUE;

+      } else {

+        isvalid = FALSE;

+      }

+

+      if (isvalid) {

+        isvalid = FALSE;

+        k = 1;

+        exitg2 = FALSE;

+        while ((exitg2 == FALSE) && (k < 625)) {

+          if (mt[k - 1] == 0U) {

+            k++;

+          } else {

+            isvalid = TRUE;

+            exitg2 = TRUE;

+          }

+        }

+      }

+

+      if (!isvalid) {

+        twister_state_vector(mt, 5489.0);

+      }

+    } else {

+      exitg1 = 1;

+    }

+  } while (exitg1 == 0);

+

+  return r;

+}

+

+static real_T eml_rand_mt19937ar(uint32_T e_state[625])

+{

+  real_T r;

+  int32_T exitg1;

+  uint32_T u32[2];

+  int32_T i;

+  static const real_T dv1[257] = { 0.0, 0.215241895984875, 0.286174591792068,

+    0.335737519214422, 0.375121332878378, 0.408389134611989, 0.43751840220787,

+    0.46363433679088, 0.487443966139235, 0.50942332960209, 0.529909720661557,

+    0.549151702327164, 0.567338257053817, 0.584616766106378, 0.601104617755991,

+    0.61689699000775, 0.63207223638606, 0.646695714894993, 0.660822574244419,

+    0.674499822837293, 0.687767892795788, 0.700661841106814, 0.713212285190975,

+    0.725446140909999, 0.737387211434295, 0.749056662017815, 0.760473406430107,

+    0.771654424224568, 0.782615023307232, 0.793369058840623, 0.80392911698997,

+    0.814306670135215, 0.824512208752291, 0.834555354086381, 0.844444954909153,

+    0.854189171008163, 0.863795545553308, 0.87327106808886, 0.882622229585165,

+    0.891855070732941, 0.900975224461221, 0.909987953496718, 0.91889818364959,

+    0.927710533401999, 0.936429340286575, 0.945058684468165, 0.953602409881086,

+    0.96206414322304, 0.970447311064224, 0.978755155294224, 0.986990747099062,

+    0.99515699963509, 1.00325667954467, 1.01129241744, 1.01926671746548,

+    1.02718196603564, 1.03504043983344, 1.04284431314415, 1.05059566459093,

+    1.05829648333067, 1.06594867476212, 1.07355406579244, 1.0811144097034,

+    1.08863139065398, 1.09610662785202, 1.10354167942464, 1.11093804601357,

+    1.11829717411934, 1.12562045921553, 1.13290924865253, 1.14016484436815,

+    1.14738850542085, 1.15458145035993, 1.16174485944561, 1.16887987673083,

+    1.17598761201545, 1.18306914268269, 1.19012551542669, 1.19715774787944,

+    1.20416683014438, 1.2111537262437, 1.21811937548548, 1.22506469375653,

+    1.23199057474614, 1.23889789110569, 1.24578749554863, 1.2526602218949,

+    1.25951688606371, 1.26635828701823, 1.27318520766536, 1.27999841571382,

+    1.28679866449324, 1.29358669373695, 1.30036323033084, 1.30712898903073,

+    1.31388467315022, 1.32063097522106, 1.32736857762793, 1.33409815321936,

+    1.3408203658964, 1.34753587118059, 1.35424531676263, 1.36094934303328,

+    1.36764858359748, 1.37434366577317, 1.38103521107586, 1.38772383568998,

+    1.39441015092814, 1.40109476367925, 1.4077782768464, 1.41446128977547,

+    1.42114439867531, 1.42782819703026, 1.43451327600589, 1.44120022484872,

+    1.44788963128058, 1.45458208188841, 1.46127816251028, 1.46797845861808,

+    1.47468355569786, 1.48139403962819, 1.48811049705745, 1.49483351578049,

+    1.50156368511546, 1.50830159628131, 1.51504784277671, 1.521803020761,

+    1.52856772943771, 1.53534257144151, 1.542128153229, 1.54892508547417,

+    1.55573398346918, 1.56255546753104, 1.56939016341512, 1.57623870273591,

+    1.58310172339603, 1.58997987002419, 1.59687379442279, 1.60378415602609,

+    1.61071162236983, 1.61765686957301, 1.62462058283303, 1.63160345693487,

+    1.63860619677555, 1.64562951790478, 1.65267414708306, 1.65974082285818,

+    1.66683029616166, 1.67394333092612, 1.68108070472517, 1.68824320943719,

+    1.69543165193456, 1.70264685479992, 1.7098896570713, 1.71716091501782,

+    1.72446150294804, 1.73179231405296, 1.73915426128591, 1.74654827828172,

+    1.75397532031767, 1.76143636531891, 1.76893241491127, 1.77646449552452,

+    1.78403365954944, 1.79164098655216, 1.79928758454972, 1.80697459135082,

+    1.81470317596628, 1.82247454009388, 1.83028991968276, 1.83815058658281,

+    1.84605785028518, 1.8540130597602, 1.86201760539967, 1.87007292107127,

+    1.878180486293, 1.88634182853678, 1.8945585256707, 1.90283220855043,

+    1.91116456377125, 1.91955733659319, 1.92801233405266, 1.93653142827569,

+    1.94511656000868, 1.95376974238465, 1.96249306494436, 1.97128869793366,

+    1.98015889690048, 1.98910600761744, 1.99813247135842, 2.00724083056053,

+    2.0164337349062, 2.02571394786385, 2.03508435372962, 2.04454796521753,

+    2.05410793165065, 2.06376754781173, 2.07353026351874, 2.0833996939983,

+    2.09337963113879, 2.10347405571488, 2.11368715068665, 2.12402331568952,

+    2.13448718284602, 2.14508363404789, 2.15581781987674, 2.16669518035431,

+    2.17772146774029, 2.18890277162636, 2.20024554661128, 2.21175664288416,

+    2.22344334009251, 2.23531338492992, 2.24737503294739, 2.25963709517379,

+    2.27210899022838, 2.28480080272449, 2.29772334890286, 2.31088825060137,

+    2.32430801887113, 2.33799614879653, 2.35196722737914, 2.36623705671729,

+    2.38082279517208, 2.39574311978193, 2.41101841390112, 2.42667098493715,

+    2.44272531820036, 2.4592083743347, 2.47614993967052, 2.49358304127105,

+    2.51154444162669, 2.53007523215985, 2.54922155032478, 2.56903545268184,

+    2.58957598670829, 2.61091051848882, 2.63311639363158, 2.65628303757674,

+    2.68051464328574, 2.70593365612306, 2.73268535904401, 2.76094400527999,

+    2.79092117400193, 2.82287739682644, 2.85713873087322, 2.89412105361341,

+    2.93436686720889, 2.97860327988184, 3.02783779176959, 3.08352613200214,

+    3.147889289518, 3.2245750520478, 3.32024473383983, 3.44927829856143,

+    3.65415288536101, 3.91075795952492 };

+

+  real_T u;

+  static const real_T dv2[257] = { 1.0, 0.977101701267673, 0.959879091800108,

+    0.9451989534423, 0.932060075959231, 0.919991505039348, 0.908726440052131,

+    0.898095921898344, 0.887984660755834, 0.878309655808918, 0.869008688036857,

+    0.860033621196332, 0.851346258458678, 0.842915653112205, 0.834716292986884,

+    0.826726833946222, 0.818929191603703, 0.811307874312656, 0.803849483170964,

+    0.796542330422959, 0.789376143566025, 0.782341832654803, 0.775431304981187,

+    0.768637315798486, 0.761953346836795, 0.755373506507096, 0.748892447219157,

+    0.742505296340151, 0.736207598126863, 0.729995264561476, 0.72386453346863,

+    0.717811932630722, 0.711834248878248, 0.705928501332754, 0.700091918136512,

+    0.694321916126117, 0.688616083004672, 0.682972161644995, 0.677388036218774,

+    0.671861719897082, 0.66639134390875, 0.660975147776663, 0.655611470579697,

+    0.650298743110817, 0.645035480820822, 0.639820277453057, 0.634651799287624,

+    0.629528779924837, 0.624450015547027, 0.619414360605834, 0.614420723888914,

+    0.609468064925773, 0.604555390697468, 0.599681752619125, 0.594846243767987,

+    0.590047996332826, 0.585286179263371, 0.580559996100791, 0.575868682972354,

+    0.571211506735253, 0.566587763256165, 0.561996775814525, 0.557437893618766,

+    0.552910490425833, 0.548413963255266, 0.543947731190026, 0.539511234256952,

+    0.535103932380458, 0.530725304403662, 0.526374847171684, 0.522052074672322,

+    0.517756517229756, 0.513487720747327, 0.509245245995748, 0.505028667943468,

+    0.500837575126149, 0.49667156905249, 0.492530263643869, 0.488413284705458,

+    0.484320269426683, 0.480250865909047, 0.476204732719506, 0.47218153846773,

+    0.468180961405694, 0.464202689048174, 0.460246417812843, 0.456311852678716,

+    0.452398706861849, 0.448506701507203, 0.444635565395739, 0.440785034665804,

+    0.436954852547985, 0.433144769112652, 0.429354541029442, 0.425583931338022,

+    0.421832709229496, 0.418100649837848, 0.414387534040891, 0.410693148270188,

+    0.407017284329473, 0.403359739221114, 0.399720314980197, 0.396098818515832,

+    0.392495061459315, 0.388908860018789, 0.385340034840077, 0.381788410873393,

+    0.378253817245619, 0.374736087137891, 0.371235057668239, 0.367750569779032,

+    0.364282468129004, 0.360830600989648, 0.357394820145781, 0.353974980800077,

+    0.350570941481406, 0.347182563956794, 0.343809713146851, 0.340452257044522,

+    0.337110066637006, 0.333783015830718, 0.330470981379163, 0.327173842813601,

+    0.323891482376391, 0.320623784956905, 0.317370638029914, 0.314131931596337,

+    0.310907558126286, 0.307697412504292, 0.30450139197665, 0.301319396100803,

+    0.298151326696685, 0.294997087799962, 0.291856585617095, 0.288729728482183,

+    0.285616426815502, 0.282516593083708, 0.279430141761638, 0.276356989295668,

+    0.273297054068577, 0.270250256365875, 0.267216518343561, 0.264195763997261,

+    0.261187919132721, 0.258192911337619, 0.255210669954662, 0.252241126055942,

+    0.249284212418529, 0.246339863501264, 0.24340801542275, 0.240488605940501,

+    0.237581574431238, 0.23468686187233, 0.231804410824339, 0.228934165414681,

+    0.226076071322381, 0.223230075763918, 0.220396127480152, 0.217574176724331,

+    0.214764175251174, 0.211966076307031, 0.209179834621125, 0.206405406397881,

+    0.203642749310335, 0.200891822494657, 0.198152586545776, 0.195425003514135,

+    0.192709036903589, 0.190004651670465, 0.187311814223801, 0.1846304924268,

+    0.181960655599523, 0.179302274522848, 0.176655321443735, 0.174019770081839,

+    0.171395595637506, 0.168782774801212, 0.166181285764482, 0.163591108232366,

+    0.161012223437511, 0.158444614155925, 0.15588826472448, 0.153343161060263,

+    0.150809290681846, 0.148286642732575, 0.145775208005994, 0.143274978973514,

+    0.140785949814445, 0.138308116448551, 0.135841476571254, 0.133386029691669,

+    0.130941777173644, 0.12850872228, 0.126086870220186, 0.123676228201597,

+    0.12127680548479, 0.11888861344291, 0.116511665625611, 0.114145977827839,

+    0.111791568163838, 0.109448457146812, 0.107116667774684, 0.104796225622487,

+    0.102487158941935, 0.10018949876881, 0.0979032790388625, 0.095628536713009,

+    0.093365311912691, 0.0911136480663738, 0.0888735920682759,

+    0.0866451944505581, 0.0844285095703535, 0.082223595813203,

+    0.0800305158146631, 0.0778493367020961, 0.0756801303589272,

+    0.0735229737139814, 0.0713779490588905, 0.0692451443970068,

+    0.0671246538277886, 0.065016577971243, 0.0629210244377582, 0.06083810834954,

+    0.0587679529209339, 0.0567106901062031, 0.0546664613248891,

+    0.0526354182767924, 0.0506177238609479, 0.0486135532158687,

+    0.0466230949019305, 0.0446465522512946, 0.0426841449164746,

+    0.0407361106559411, 0.0388027074045262, 0.0368842156885674,

+    0.0349809414617162, 0.0330932194585786, 0.0312214171919203,

+    0.0293659397581334, 0.0275272356696031, 0.0257058040085489,

+    0.0239022033057959, 0.0221170627073089, 0.0203510962300445,

+    0.0186051212757247, 0.0168800831525432, 0.0151770883079353,

+    0.0134974506017399, 0.0118427578579079, 0.0102149714397015,

+    0.00861658276939875, 0.00705087547137324, 0.00552240329925101,

+    0.00403797259336304, 0.00260907274610216, 0.0012602859304986,

+    0.000477467764609386 };

+

+  real_T x;

+  do {

+    exitg1 = 0;

+    genrand_uint32_vector(e_state, u32);

+    i = (int32_T)((u32[1] >> 24U) + 1U);

+    r = (((real_T)(u32[0] >> 3U) * 1.6777216E+7 + (real_T)((int32_T)u32[1] &

+           16777215)) * 2.2204460492503131E-16 - 1.0) * dv1[i];

+    if (fabs(r) <= dv1[i - 1]) {

+      exitg1 = 1;

+    } else if (i < 256) {

+      u = b_genrandu(e_state);

+      if (dv2[i] + u * (dv2[i - 1] - dv2[i]) < exp(-0.5 * r * r)) {

+        exitg1 = 1;

+      }

+    } else {

+      do {

+        u = b_genrandu(e_state);

+        x = log(u) * 0.273661237329758;

+        u = b_genrandu(e_state);

+      } while (!(-2.0 * log(u) > x * x));

+

+      if (r < 0.0) {

+        r = x - 3.65415288536101;

+      } else {

+        r = 3.65415288536101 - x;

+      }

+

+      exitg1 = 1;

+    }

+  } while (exitg1 == 0);

+

+  return r;

+}

+

+static real_T eml_rand_shr3cong(uint32_T e_state[2])

+{

+  real_T r;

+  uint32_T icng;

+  uint32_T jsr;

+  uint32_T ui;

+  int32_T j;

+  static const real_T dv0[65] = { 0.340945, 0.4573146, 0.5397793, 0.6062427,

+    0.6631691, 0.7136975, 0.7596125, 0.8020356, 0.8417227, 0.8792102, 0.9148948,

+    0.9490791, 0.9820005, 1.0138492, 1.044781, 1.0749254, 1.1043917, 1.1332738,

+    1.161653, 1.189601, 1.2171815, 1.2444516, 1.2714635, 1.298265, 1.3249008,

+    1.3514125, 1.3778399, 1.4042211, 1.4305929, 1.4569915, 1.4834527, 1.5100122,

+    1.5367061, 1.5635712, 1.5906454, 1.617968, 1.6455802, 1.6735255, 1.7018503,

+    1.7306045, 1.7598422, 1.7896223, 1.8200099, 1.851077, 1.8829044, 1.9155831,

+    1.9492166, 1.9839239, 2.0198431, 2.0571356, 2.095993, 2.136645, 2.1793713,

+    2.2245175, 2.2725186, 2.3239338, 2.3795008, 2.4402218, 2.5075117, 2.5834658,

+    2.6713916, 2.7769942, 2.7769942, 2.7769942, 2.7769942 };

+

+  real_T x;

+  real_T y;

+  real_T s;

+  icng = 69069U * e_state[0] + 1234567U;

+  jsr = e_state[1] ^ e_state[1] << 13U;

+  jsr ^= jsr >> 17U;

+  jsr ^= jsr << 5U;

+  ui = icng + jsr;

+  j = (int32_T)(ui & 63U);

+  r = (real_T)(int32_T)ui * 4.6566128730773926E-10 * dv0[j + 1];

+  if (fabs(r) <= dv0[j]) {

+  } else {

+    x = (fabs(r) - dv0[j]) / (dv0[j + 1] - dv0[j]);

+    icng = 69069U * icng + 1234567U;

+    jsr ^= jsr << 13U;

+    jsr ^= jsr >> 17U;

+    jsr ^= jsr << 5U;

+    y = (real_T)(int32_T)(icng + jsr) * 2.328306436538696E-10;

+    s = x + (0.5 + y);

+    if (s > 1.301198) {

+      if (r < 0.0) {

+        r = 0.4878992 * x - 0.4878992;

+      } else {

+        r = 0.4878992 - 0.4878992 * x;

+      }

+    } else if (s <= 0.9689279) {

+    } else {

+      s = 0.4878992 * x;

+      x = 0.4878992 - 0.4878992 * x;

+      if (0.5 + y > 12.67706 - 12.37586 * exp(-0.5 * (0.4878992 - s) * x)) {

+        if (r < 0.0) {

+          r = -(0.4878992 - s);

+        } else {

+          r = 0.4878992 - s;

+        }

+      } else if (exp(-0.5 * dv0[j + 1] * dv0[j + 1]) + (0.5 + y) * 0.01958303 /

+                 dv0[j + 1] <= exp(-0.5 * r * r)) {

+      } else {

+        do {

+          icng = 69069U * icng + 1234567U;

+          jsr ^= jsr << 13U;

+          jsr ^= jsr >> 17U;

+          jsr ^= jsr << 5U;

+          x = log(0.5 + (real_T)(int32_T)(icng + jsr) * 2.328306436538696E-10) /

+            2.776994;

+          icng = 69069U * icng + 1234567U;

+          jsr ^= jsr << 13U;

+          jsr ^= jsr >> 17U;

+          jsr ^= jsr << 5U;

+        } while (!(-2.0 * log(0.5 + (real_T)(int32_T)(icng + jsr) *

+                              2.328306436538696E-10) > x * x));

+

+        if (r < 0.0) {

+          r = x - 2.776994;

+        } else {

+          r = 2.776994 - x;

+        }

+      }

+    }

+  }

+

+  e_state[0] = icng;

+  e_state[1] = jsr;

+  return r;

+}

+

+static void genrand_uint32_vector(uint32_T mt[625], uint32_T u[2])

+{

+  int32_T i;

+  uint32_T mti;

+  int32_T kk;

+  uint32_T y;

+  uint32_T b_y;

+  uint32_T c_y;

+  uint32_T d_y;

+  for (i = 0; i < 2; i++) {

+    u[i] = 0U;

+  }

+

+  for (i = 0; i < 2; i++) {

+    mti = mt[624] + 1U;

+    if (mti >= 625U) {

+      for (kk = 0; kk < 227; kk++) {

+        y = (mt[kk] & 2147483648U) | (mt[1 + kk] & 2147483647U);

+        if ((int32_T)(y & 1U) == 0) {

+          b_y = y >> 1U;

+        } else {

+          b_y = y >> 1U ^ 2567483615U;

+        }

+

+        mt[kk] = mt[397 + kk] ^ b_y;

+      }

+

+      for (kk = 0; kk < 396; kk++) {

+        y = (mt[227 + kk] & 2147483648U) | (mt[228 + kk] & 2147483647U);

+        if ((int32_T)(y & 1U) == 0) {

+          c_y = y >> 1U;

+        } else {

+          c_y = y >> 1U ^ 2567483615U;

+        }

+

+        mt[227 + kk] = mt[kk] ^ c_y;

+      }

+

+      y = (mt[623] & 2147483648U) | (mt[0] & 2147483647U);

+      if ((int32_T)(y & 1U) == 0) {

+        d_y = y >> 1U;

+      } else {

+        d_y = y >> 1U ^ 2567483615U;

+      }

+

+      mt[623] = mt[396] ^ d_y;

+      mti = 1U;

+    }

+

+    y = mt[(int32_T)mti - 1];

+    mt[624] = mti;

+    y ^= y >> 11U;

+    y ^= y << 7U & 2636928640U;

+    y ^= y << 15U & 4022730752U;

+    y ^= y >> 18U;

+    u[i] = y;

+  }

+}

+

+static void genrandu(uint32_T s, uint32_T *e_state, real_T *r)

+{

+  int32_T hi;

+  uint32_T test1;

+  uint32_T test2;

+  hi = (int32_T)(s / 127773U);

+  test1 = 16807U * (s - (uint32_T)hi * 127773U);

+  test2 = 2836U * (uint32_T)hi;

+  if (test1 < test2) {

+    *e_state = (test1 - test2) + 2147483647U;

+  } else {

+    *e_state = test1 - test2;

+  }

+

+  *r = (real_T)*e_state * 4.6566128752457969E-10;

+}

+

+static void twister_state_vector(uint32_T mt[625], real_T seed)

+{

+  uint32_T r;

+  int32_T mti;

+  if (seed < 4.294967296E+9) {

+    if (seed >= 0.0) {

+      r = (uint32_T)seed;

+    } else {

+      r = 0U;

+    }

+  } else if (seed >= 4.294967296E+9) {

+    r = MAX_uint32_T;

+  } else {

+    r = 0U;

+  }

+

+  mt[0] = r;

+  for (mti = 0; mti < 623; mti++) {

+    r = (r ^ r >> 30U) * 1812433253U + (uint32_T)(1 + mti);

+    mt[1 + mti] = r;

+  }

+

+  mt[624] = 624U;

+}

+

+real_T randn(void)

+{

+  real_T r;

+  uint32_T e_state;

+  real_T t;

+  real_T b_r;

+  uint32_T f_state;

+  if (method == 0U) {

+    if (b_method == 4U) {

+      do {

+        genrandu(b_state, &e_state, &r);

+        genrandu(e_state, &b_state, &t);

+        b_r = 2.0 * r - 1.0;

+        t = 2.0 * t - 1.0;

+        t = t * t + b_r * b_r;

+      } while (!(t <= 1.0));

+

+      r = (2.0 * r - 1.0) * sqrt(-2.0 * log(t) / t);

+    } else if (b_method == 5U) {

+      r = eml_rand_shr3cong(c_state);

+    } else {

+      if (!state_not_empty) {

+        memset(&d_state[0], 0, 625U * sizeof(uint32_T));

+        twister_state_vector(d_state, 5489.0);

+        state_not_empty = TRUE;

+      }

+

+      r = eml_rand_mt19937ar(d_state);

+    }

+  } else if (method == 4U) {

+    e_state = state[0];

+    do {

+      genrandu(e_state, &f_state, &r);

+      genrandu(f_state, &e_state, &t);

+      b_r = 2.0 * r - 1.0;

+      t = 2.0 * t - 1.0;

+      t = t * t + b_r * b_r;

+    } while (!(t <= 1.0));

+

+    state[0] = e_state;

+    r = (2.0 * r - 1.0) * sqrt(-2.0 * log(t) / t);

+  } else {

+    r = eml_rand_shr3cong(state);

+  }

+

+  return r;

+}

+

+/* End of code generation (randn.c) */

diff --git a/apps/position_estimator_mc/codegen/randn.h b/apps/position_estimator_mc/codegen/randn.h
new file mode 100755
index 000000000..8a2aa9277
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/randn.h
@@ -0,0 +1,33 @@
+/*

+ * randn.h

+ *

+ * Code generation for function 'randn'

+ *

+ * C source code generated on: Tue Feb 19 15:26:32 2013

+ *

+ */

+

+#ifndef __RANDN_H__

+#define __RANDN_H__

+/* Include files */

+#include <math.h>

+#include <stddef.h>

+#include <stdlib.h>

+#include <string.h>

+#include "rt_nonfinite.h"

+

+#include "rtwtypes.h"

+#include "kalman_dlqe3_types.h"

+

+/* Type Definitions */

+

+/* Named Constants */

+

+/* Variable Declarations */

+

+/* Variable Definitions */

+

+/* Function Declarations */

+extern real_T randn(void);

+#endif

+/* End of code generation (randn.h) */

diff --git a/apps/position_estimator_mc/codegen/rtGetInf.c b/apps/position_estimator_mc/codegen/rtGetInf.c
new file mode 100755
index 000000000..c6fa7884e
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/rtGetInf.c
@@ -0,0 +1,139 @@
+/*
+ * rtGetInf.c
+ *
+ * Code generation for function 'kalman_dlqe2'
+ *
+ * C source code generated on: Thu Feb 14 12:52:29 2013
+ *
+ */
+
+/*
+ * Abstract:
+ *       MATLAB for code generation function to initialize non-finite, Inf and MinusInf
+ */
+#include "rtGetInf.h"
+#define NumBitsPerChar	8U
+
+/* Function: rtGetInf ==================================================
+ * Abstract:
+ * Initialize rtInf needed by the generated code.
+ * Inf is initialized as non-signaling. Assumes IEEE.
+ */
+real_T rtGetInf(void)
+{
+  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
+  real_T inf = 0.0;
+  if (bitsPerReal == 32U) {
+    inf = rtGetInfF();
+  } else {
+    uint16_T one = 1U;
+    enum {
+      LittleEndian,
+      BigEndian
+    } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
+    switch (machByteOrder) {
+     case LittleEndian:
+      {
+        union {
+          LittleEndianIEEEDouble bitVal;
+          real_T fltVal;
+        } tmpVal;
+
+        tmpVal.bitVal.words.wordH = 0x7FF00000U;
+        tmpVal.bitVal.words.wordL = 0x00000000U;
+        inf = tmpVal.fltVal;
+        break;
+      }
+
+     case BigEndian:
+      {
+        union {
+          BigEndianIEEEDouble bitVal;
+          real_T fltVal;
+        } tmpVal;
+
+        tmpVal.bitVal.words.wordH = 0x7FF00000U;
+        tmpVal.bitVal.words.wordL = 0x00000000U;
+        inf = tmpVal.fltVal;
+        break;
+      }
+    }
+  }
+
+  return inf;
+}
+
+/* Function: rtGetInfF ==================================================
+ * Abstract:
+ * Initialize rtInfF needed by the generated code.
+ * Inf is initialized as non-signaling. Assumes IEEE.
+ */
+real32_T rtGetInfF(void)
+{
+  IEEESingle infF;
+  infF.wordL.wordLuint = 0x7F800000U;
+  return infF.wordL.wordLreal;
+}
+
+/* Function: rtGetMinusInf ==================================================
+ * Abstract:
+ * Initialize rtMinusInf needed by the generated code.
+ * Inf is initialized as non-signaling. Assumes IEEE.
+ */
+real_T rtGetMinusInf(void)
+{
+  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
+  real_T minf = 0.0;
+  if (bitsPerReal == 32U) {
+    minf = rtGetMinusInfF();
+  } else {
+    uint16_T one = 1U;
+    enum {
+      LittleEndian,
+      BigEndian
+    } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
+    switch (machByteOrder) {
+     case LittleEndian:
+      {
+        union {
+          LittleEndianIEEEDouble bitVal;
+          real_T fltVal;
+        } tmpVal;
+
+        tmpVal.bitVal.words.wordH = 0xFFF00000U;
+        tmpVal.bitVal.words.wordL = 0x00000000U;
+        minf = tmpVal.fltVal;
+        break;
+      }
+
+     case BigEndian:
+      {
+        union {
+          BigEndianIEEEDouble bitVal;
+          real_T fltVal;
+        } tmpVal;
+
+        tmpVal.bitVal.words.wordH = 0xFFF00000U;
+        tmpVal.bitVal.words.wordL = 0x00000000U;
+        minf = tmpVal.fltVal;
+        break;
+      }
+    }
+  }
+
+  return minf;
+}
+
+/* Function: rtGetMinusInfF ==================================================
+ * Abstract:
+ * Initialize rtMinusInfF needed by the generated code.
+ * Inf is initialized as non-signaling. Assumes IEEE.
+ */
+real32_T rtGetMinusInfF(void)
+{
+  IEEESingle minfF;
+  minfF.wordL.wordLuint = 0xFF800000U;
+  return minfF.wordL.wordLreal;
+}
+
+/* End of code generation (rtGetInf.c) */
diff --git a/apps/position_estimator_mc/codegen/rtGetInf.h b/apps/position_estimator_mc/codegen/rtGetInf.h
new file mode 100755
index 000000000..e7b2a2d1c
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/rtGetInf.h
@@ -0,0 +1,23 @@
+/*
+ * rtGetInf.h
+ *
+ * Code generation for function 'kalman_dlqe2'
+ *
+ * C source code generated on: Thu Feb 14 12:52:29 2013
+ *
+ */
+
+#ifndef __RTGETINF_H__
+#define __RTGETINF_H__
+
+#include <stddef.h>
+#include "rtwtypes.h"
+#include "rt_nonfinite.h"
+
+extern real_T rtGetInf(void);
+extern real32_T rtGetInfF(void);
+extern real_T rtGetMinusInf(void);
+extern real32_T rtGetMinusInfF(void);
+
+#endif
+/* End of code generation (rtGetInf.h) */
diff --git a/apps/position_estimator_mc/codegen/rtGetNaN.c b/apps/position_estimator_mc/codegen/rtGetNaN.c
new file mode 100755
index 000000000..552770149
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/rtGetNaN.c
@@ -0,0 +1,96 @@
+/*
+ * rtGetNaN.c
+ *
+ * Code generation for function 'kalman_dlqe2'
+ *
+ * C source code generated on: Thu Feb 14 12:52:29 2013
+ *
+ */
+
+/*
+ * Abstract:
+ *       MATLAB for code generation function to initialize non-finite, NaN
+ */
+#include "rtGetNaN.h"
+#define NumBitsPerChar	8U
+
+/* Function: rtGetNaN ==================================================
+ * Abstract:
+ * Initialize rtNaN needed by the generated code.
+ * NaN is initialized as non-signaling. Assumes IEEE.
+ */
+real_T rtGetNaN(void)
+{
+  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
+  real_T nan = 0.0;
+  if (bitsPerReal == 32U) {
+    nan = rtGetNaNF();
+  } else {
+  uint16_T one = 1U;
+  enum {
+    LittleEndian,
+    BigEndian
+  } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
+  switch (machByteOrder) {
+   case LittleEndian:
+    {
+      union {
+        LittleEndianIEEEDouble bitVal;
+        real_T fltVal;
+      } tmpVal;
+
+      tmpVal.bitVal.words.wordH = 0xFFF80000U;
+      tmpVal.bitVal.words.wordL = 0x00000000U;
+      nan = tmpVal.fltVal;
+      break;
+    }
+
+   case BigEndian:
+    {
+        union {
+          BigEndianIEEEDouble bitVal;
+          real_T fltVal;
+        } tmpVal;
+
+        tmpVal.bitVal.words.wordH = 0x7FFFFFFFU;
+        tmpVal.bitVal.words.wordL = 0xFFFFFFFFU;
+        nan = tmpVal.fltVal;
+        break;
+      }
+    }
+  }
+
+  return nan;
+}
+
+/* Function: rtGetNaNF ==================================================
+ * Abstract:
+ * Initialize rtNaNF needed by the generated code.
+ * NaN is initialized as non-signaling. Assumes IEEE.
+ */
+real32_T rtGetNaNF(void)
+{
+  IEEESingle nanF = { { 0 } };
+  uint16_T one = 1U;
+  enum {
+    LittleEndian,
+    BigEndian
+  } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
+  switch (machByteOrder) {
+   case LittleEndian:
+    {
+      nanF.wordL.wordLuint = 0xFFC00000U;
+      break;
+    }
+
+   case BigEndian:
+    {
+      nanF.wordL.wordLuint = 0x7FFFFFFFU;
+      break;
+    }
+  }
+
+  return nanF.wordL.wordLreal;
+}
+
+/* End of code generation (rtGetNaN.c) */
diff --git a/apps/position_estimator_mc/codegen/rtGetNaN.h b/apps/position_estimator_mc/codegen/rtGetNaN.h
new file mode 100755
index 000000000..5acdd9790
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/rtGetNaN.h
@@ -0,0 +1,21 @@
+/*
+ * rtGetNaN.h
+ *
+ * Code generation for function 'kalman_dlqe2'
+ *
+ * C source code generated on: Thu Feb 14 12:52:29 2013
+ *
+ */
+
+#ifndef __RTGETNAN_H__
+#define __RTGETNAN_H__
+
+#include <stddef.h>
+#include "rtwtypes.h"
+#include "rt_nonfinite.h"
+
+extern real_T rtGetNaN(void);
+extern real32_T rtGetNaNF(void);
+
+#endif
+/* End of code generation (rtGetNaN.h) */
diff --git a/apps/position_estimator_mc/codegen/rt_nonfinite.c b/apps/position_estimator_mc/codegen/rt_nonfinite.c
new file mode 100755
index 000000000..de121c4a0
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/rt_nonfinite.c
@@ -0,0 +1,87 @@
+/*
+ * rt_nonfinite.c
+ *
+ * Code generation for function 'kalman_dlqe2'
+ *
+ * C source code generated on: Thu Feb 14 12:52:29 2013
+ *
+ */
+
+/*
+ * Abstract:
+ *      MATLAB for code generation function to initialize non-finites,
+ *      (Inf, NaN and -Inf).
+ */
+#include "rt_nonfinite.h"
+#include "rtGetNaN.h"
+#include "rtGetInf.h"
+
+real_T rtInf;
+real_T rtMinusInf;
+real_T rtNaN;
+real32_T rtInfF;
+real32_T rtMinusInfF;
+real32_T rtNaNF;
+
+/* Function: rt_InitInfAndNaN ==================================================
+ * Abstract:
+ * Initialize the rtInf, rtMinusInf, and rtNaN needed by the
+ * generated code. NaN is initialized as non-signaling. Assumes IEEE.
+ */
+void rt_InitInfAndNaN(size_t realSize)
+{
+  (void) (realSize);
+  rtNaN = rtGetNaN();
+  rtNaNF = rtGetNaNF();
+  rtInf = rtGetInf();
+  rtInfF = rtGetInfF();
+  rtMinusInf = rtGetMinusInf();
+  rtMinusInfF = rtGetMinusInfF();
+}
+
+/* Function: rtIsInf ==================================================
+ * Abstract:
+ * Test if value is infinite
+ */
+boolean_T rtIsInf(real_T value)
+{
+  return ((value==rtInf || value==rtMinusInf) ? 1U : 0U);
+}
+
+/* Function: rtIsInfF =================================================
+ * Abstract:
+ * Test if single-precision value is infinite
+ */
+boolean_T rtIsInfF(real32_T value)
+{
+  return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U);
+}
+
+/* Function: rtIsNaN ==================================================
+ * Abstract:
+ * Test if value is not a number
+ */
+boolean_T rtIsNaN(real_T value)
+{
+#if defined(_MSC_VER) && (_MSC_VER <= 1200)
+  return _isnan(value)? TRUE:FALSE;
+#else
+  return (value!=value)? 1U:0U;
+#endif
+}
+
+/* Function: rtIsNaNF =================================================
+ * Abstract:
+ * Test if single-precision value is not a number
+ */
+boolean_T rtIsNaNF(real32_T value)
+{
+#if defined(_MSC_VER) && (_MSC_VER <= 1200)
+  return _isnan((real_T)value)? true:false;
+#else
+  return (value!=value)? 1U:0U;
+#endif
+}
+
+
+/* End of code generation (rt_nonfinite.c) */
diff --git a/apps/position_estimator_mc/codegen/rt_nonfinite.h b/apps/position_estimator_mc/codegen/rt_nonfinite.h
new file mode 100755
index 000000000..3bbcfd087
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/rt_nonfinite.h
@@ -0,0 +1,53 @@
+/*
+ * rt_nonfinite.h
+ *
+ * Code generation for function 'kalman_dlqe2'
+ *
+ * C source code generated on: Thu Feb 14 12:52:29 2013
+ *
+ */
+
+#ifndef __RT_NONFINITE_H__
+#define __RT_NONFINITE_H__
+
+#if defined(_MSC_VER) && (_MSC_VER <= 1200)
+#include <float.h>
+#endif
+#include <stddef.h>
+#include "rtwtypes.h"
+
+extern real_T rtInf;
+extern real_T rtMinusInf;
+extern real_T rtNaN;
+extern real32_T rtInfF;
+extern real32_T rtMinusInfF;
+extern real32_T rtNaNF;
+extern void rt_InitInfAndNaN(size_t realSize);
+extern boolean_T rtIsInf(real_T value);
+extern boolean_T rtIsInfF(real32_T value);
+extern boolean_T rtIsNaN(real_T value);
+extern boolean_T rtIsNaNF(real32_T value);
+
+typedef struct {
+  struct {
+    uint32_T wordH;
+    uint32_T wordL;
+  } words;
+} BigEndianIEEEDouble;
+
+typedef struct {
+  struct {
+    uint32_T wordL;
+    uint32_T wordH;
+  } words;
+} LittleEndianIEEEDouble;
+
+typedef struct {
+  union {
+    real32_T wordLreal;
+    uint32_T wordLuint;
+  } wordL;
+} IEEESingle;
+
+#endif
+/* End of code generation (rt_nonfinite.h) */
diff --git a/apps/position_estimator_mc/codegen/rtwtypes.h b/apps/position_estimator_mc/codegen/rtwtypes.h
new file mode 100755
index 000000000..8916e8572
--- /dev/null
+++ b/apps/position_estimator_mc/codegen/rtwtypes.h
@@ -0,0 +1,159 @@
+/*
+ * rtwtypes.h
+ *
+ * Code generation for function 'kalman_dlqe2'
+ *
+ * C source code generated on: Thu Feb 14 12:52:29 2013
+ *
+ */
+
+#ifndef __RTWTYPES_H__
+#define __RTWTYPES_H__
+#ifndef TRUE
+# define TRUE (1U)
+#endif
+#ifndef FALSE
+# define FALSE (0U)
+#endif
+#ifndef __TMWTYPES__
+#define __TMWTYPES__
+
+#include <limits.h>
+
+/*=======================================================================* 
+ * Target hardware information
+ *   Device type: Generic->MATLAB Host Computer
+ *   Number of bits:     char:   8    short:   16    int:  32
+ *                       long:  32      native word size:  32
+ *   Byte ordering: LittleEndian
+ *   Signed integer division rounds to: Undefined
+ *   Shift right on a signed integer as arithmetic shift: off
+ *=======================================================================*/
+
+/*=======================================================================* 
+ * Fixed width word size data types:                                     * 
+ *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     * 
+ *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   * 
+ *   real32_T, real64_T           - 32 and 64 bit floating point numbers * 
+ *=======================================================================*/
+
+typedef signed char int8_T;
+typedef unsigned char uint8_T;
+typedef short int16_T;
+typedef unsigned short uint16_T;
+typedef int int32_T;
+typedef unsigned int uint32_T;
+typedef float real32_T;
+typedef double real64_T;
+
+/*===========================================================================* 
+ * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T,       * 
+ *                           ulong_T, char_T and byte_T.                     * 
+ *===========================================================================*/
+
+typedef double real_T;
+typedef double time_T;
+typedef unsigned char boolean_T;
+typedef int int_T;
+typedef unsigned int uint_T;
+typedef unsigned long ulong_T;
+typedef char char_T;
+typedef char_T byte_T;
+
+/*===========================================================================* 
+ * Complex number type definitions                                           * 
+ *===========================================================================*/
+#define CREAL_T	
+   typedef struct {  
+     real32_T re;  
+     real32_T im;  
+   } creal32_T;  
+
+   typedef struct {  
+     real64_T re;  
+     real64_T im;  
+   } creal64_T;  
+
+   typedef struct {  
+     real_T re;  
+     real_T im;  
+   } creal_T;  
+
+   typedef struct {  
+     int8_T re;  
+     int8_T im;  
+   } cint8_T;  
+
+   typedef struct {  
+     uint8_T re;  
+     uint8_T im;  
+   } cuint8_T;  
+
+   typedef struct {  
+     int16_T re;  
+     int16_T im;  
+   } cint16_T;  
+
+   typedef struct {  
+     uint16_T re;  
+     uint16_T im;  
+   } cuint16_T;  
+
+   typedef struct {  
+     int32_T re;  
+     int32_T im;  
+   } cint32_T;  
+
+   typedef struct {  
+     uint32_T re;  
+     uint32_T im;  
+   } cuint32_T;  
+
+
+/*=======================================================================* 
+ * Min and Max:                                                          * 
+ *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     * 
+ *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   * 
+ *=======================================================================*/
+
+#define MAX_int8_T  	((int8_T)(127))
+#define MIN_int8_T  	((int8_T)(-128))
+#define MAX_uint8_T 	((uint8_T)(255))
+#define MIN_uint8_T 	((uint8_T)(0))
+#define MAX_int16_T 	((int16_T)(32767))
+#define MIN_int16_T 	((int16_T)(-32768))
+#define MAX_uint16_T	((uint16_T)(65535))
+#define MIN_uint16_T	((uint16_T)(0))
+#define MAX_int32_T 	((int32_T)(2147483647))
+#define MIN_int32_T 	((int32_T)(-2147483647-1))
+#define MAX_uint32_T	((uint32_T)(0xFFFFFFFFU))
+#define MIN_uint32_T	((uint32_T)(0))
+
+/* Logical type definitions */
+#if !defined(__cplusplus) && !defined(__true_false_are_keywords)
+#  ifndef false
+#   define false (0U)
+#  endif
+#  ifndef true
+#   define true (1U)
+#  endif
+#endif
+
+/*
+ * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation
+ * for signed integer values.
+ */
+#if ((SCHAR_MIN + 1) != -SCHAR_MAX)
+#error "This code must be compiled using a 2's complement representation for signed integer values"
+#endif
+
+/*
+ * Maximum length of a MATLAB identifier (function/variable)
+ * including the null-termination character. Referenced by
+ * rt_logging.c and rt_matrx.c.
+ */
+#define TMW_NAME_LENGTH_MAX	64
+
+#endif
+#endif
+/* End of code generation (rtwtypes.h) */
diff --git a/apps/position_estimator_mc/kalman_dlqe1.m b/apps/position_estimator_mc/kalman_dlqe1.m
new file mode 100755
index 000000000..ff939d029
--- /dev/null
+++ b/apps/position_estimator_mc/kalman_dlqe1.m
@@ -0,0 +1,3 @@
+function [x_aposteriori] = kalman_dlqe1(A,C,K,x_aposteriori_k,z)
+    x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k);      
+end
\ No newline at end of file
diff --git a/apps/position_estimator_mc/kalman_dlqe2.m b/apps/position_estimator_mc/kalman_dlqe2.m
new file mode 100755
index 000000000..2a50164ef
--- /dev/null
+++ b/apps/position_estimator_mc/kalman_dlqe2.m
@@ -0,0 +1,9 @@
+function [x_aposteriori] = kalman_dlqe2(dt,k1,k2,k3,x_aposteriori_k,z)
+    st = 1/2*dt^2;
+    A = [1,dt,st;
+        0,1,dt;
+        0,0,1];
+    C=[1,0,0];
+    K = [k1;k2;k3];
+    x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k);      
+end
\ No newline at end of file
diff --git a/apps/position_estimator_mc/kalman_dlqe3.m b/apps/position_estimator_mc/kalman_dlqe3.m
new file mode 100755
index 000000000..4c6421b7f
--- /dev/null
+++ b/apps/position_estimator_mc/kalman_dlqe3.m
@@ -0,0 +1,17 @@
+function [x_aposteriori] = kalman_dlqe3(dt,k1,k2,k3,x_aposteriori_k,z,posUpdate,addNoise,sigma)

+    st = 1/2*dt^2;

+    A = [1,dt,st;

+        0,1,dt;

+        0,0,1];

+    C=[1,0,0];

+    K = [k1;k2;k3];

+    if addNoise==1

+        noise = sigma*randn(1,1);

+        z = z + noise;

+    end

+    if(posUpdate)

+        x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k);

+    else

+        x_aposteriori=A*x_aposteriori_k;

+    end

+end
\ No newline at end of file
diff --git a/apps/position_estimator_mc/positionKalmanFilter1D.m b/apps/position_estimator_mc/positionKalmanFilter1D.m
new file mode 100755
index 000000000..144ff8c7c
--- /dev/null
+++ b/apps/position_estimator_mc/positionKalmanFilter1D.m
@@ -0,0 +1,19 @@
+function [x_aposteriori,P_aposteriori]=positionKalmanFilter1D(A,B,C,x_aposteriori_k,P_aposteriori_k,u,z,gps_update,Q,R,thresh,decay)
+%prediction
+    x_apriori=A*x_aposteriori_k+B*u;
+    P_apriori=A*P_aposteriori_k*A'+Q;
+    if abs(u)<thresh
+        x_apriori(2)=decay*x_apriori(2);
+    end
+    %update
+    if gps_update==1
+        y=z-C*x_apriori;
+        S=C*P_apriori*C'+R;
+        K=(P_apriori*C')/S;
+        x_aposteriori=x_apriori+K*y;
+        P_aposteriori=(eye(size(P_apriori))-K*C)*P_apriori;
+    else
+        x_aposteriori=x_apriori;
+        P_aposteriori=P_apriori;
+    end
+end
diff --git a/apps/position_estimator_mc/positionKalmanFilter1D_dT.m b/apps/position_estimator_mc/positionKalmanFilter1D_dT.m
new file mode 100755
index 000000000..f94cce1fb
--- /dev/null
+++ b/apps/position_estimator_mc/positionKalmanFilter1D_dT.m
@@ -0,0 +1,26 @@
+function  [x_aposteriori,P_aposteriori]=positionKalmanFilter1D_dT(dT,x_aposteriori_k,P_aposteriori_k,u,z,gps_update,Q,R,thresh,decay)
+    %dynamics
+    A = [1 dT -0.5*dT*dT;
+         0 1 -dT;
+         0 0 1];
+    B = [0.5*dT*dT; dT; 0];
+    C = [1 0 0];
+    %prediction
+    x_apriori=A*x_aposteriori_k+B*u;
+    P_apriori=A*P_aposteriori_k*A'+Q;
+    if abs(u)<thresh
+        x_apriori(2)=decay*x_apriori(2);
+    end
+    %update
+    if gps_update==1
+        y=z-C*x_apriori;
+        S=C*P_apriori*C'+R;
+        K=(P_apriori*C')/S;
+        x_aposteriori=x_apriori+K*y;
+        P_aposteriori=(eye(size(P_apriori))-K*C)*P_apriori;
+    else
+        x_aposteriori=x_apriori;
+        P_aposteriori=P_apriori;
+    end
+end
+
diff --git a/apps/position_estimator_mc/position_estimator_mc_main.c b/apps/position_estimator_mc/position_estimator_mc_main.c
new file mode 100755
index 000000000..10dee3f22
--- /dev/null
+++ b/apps/position_estimator_mc/position_estimator_mc_main.c
@@ -0,0 +1,510 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: 	Damian Aregger	<daregger@student.ethz.ch>
+ *   			Tobias Naegeli <naegelit@student.ethz.ch>
+* 				Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file position_estimator_main.c
+ * Model-identification based position estimator for multirotors
+ */
+
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <float.h>
+#include <string.h>
+#include <nuttx/config.h>
+#include <nuttx/sched.h>
+#include <sys/prctl.h>
+#include <termios.h>
+#include <errno.h>
+#include <limits.h>
+#include <math.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_controls_effective.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_vicon_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <mavlink/mavlink_log.h>
+#include <poll.h>
+#include <systemlib/geo/geo.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+#include <drivers/drv_hrt.h>
+
+#include "position_estimator_mc_params.h"
+//#include <uORB/topics/debug_key_value.h>
+#include "codegen/kalman_dlqe2.h"
+#include "codegen/kalman_dlqe2_initialize.h"
+#include "codegen/kalman_dlqe3.h"
+#include "codegen/kalman_dlqe3_initialize.h"
+
+static bool thread_should_exit = false;	/**< Deamon exit flag */
+static bool thread_running = false;	/**< Deamon status flag */
+static int position_estimator_mc_task;	/**< Handle of deamon task / thread */
+
+__EXPORT int position_estimator_mc_main(int argc, char *argv[]);
+
+int position_estimator_mc_thread_main(int argc, char *argv[]);
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+static void
+usage(const char *reason)
+{
+	if (reason)
+		warnx("%s\n", reason);
+	warnx("usage: position_estimator_mc {start|stop|status}");
+	exit(1);
+}
+
+/**
+ * The position_estimator_mc_thread only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ *
+ * The actual stack size should be set in the call
+ * to task_create().
+ */
+int position_estimator_mc_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			printf("position_estimator_mc already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		position_estimator_mc_task = task_spawn("position_estimator_mc",
+					 SCHED_RR,
+					 SCHED_PRIORITY_MAX - 5,
+					 4096,
+					 position_estimator_mc_thread_main,
+					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		exit(0);
+	}
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("position_estimator_mc is running");
+		} else {
+			warnx("position_estimator_mc not started");
+		}
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+/****************************************************************************
+ * main
+ ****************************************************************************/
+int position_estimator_mc_thread_main(int argc, char *argv[])
+{
+	/* welcome user */
+	warnx("[position_estimator_mc] started");
+	int mavlink_fd;
+	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	mavlink_log_info(mavlink_fd, "[position_estimator_mc] started");
+
+	/* initialize values */
+	float z[3] = {0, 0, 0}; /* output variables from tangent plane mapping */
+	// float rotMatrix[4] = {1.0f,  0.0f, 0.0f,  1.0f};
+	float x_x_aposteriori_k[3] = {1.0f, 0.0f, 0.0f};
+	float x_y_aposteriori_k[3] = {1.0f, 0.0f, 0.0f};
+	float x_z_aposteriori_k[3] = {1.0f, 0.0f, 0.0f};
+	float x_x_aposteriori[3] = {0.0f, 0.0f, 0.0f};
+	float x_y_aposteriori[3] = {1.0f, 0.0f, 0.0f};
+	float x_z_aposteriori[3] = {1.0f, 0.0f, 0.0f};
+
+	// XXX this is terribly wrong and should actual dT instead
+	const float dT_const_50 = 1.0f/50.0f;
+
+	float addNoise = 0.0f;
+	float sigma = 0.0f;
+	//computed from dlqe in matlab
+	const float K_vicon_50Hz[3] = {0.5297f, 0.9873f, 0.9201f};
+	// XXX implement baro filter
+	const float K_baro[3] = {0.0248f, 0.0377f, 0.0287f};
+	float K[3] = {0.0f, 0.0f, 0.0f};
+	int baro_loop_cnt = 0;
+	int baro_loop_end = 70; /* measurement for 1 second */
+	float p0_Pa = 0.0f; /* to determin while start up */
+	float rho0 = 1.293f; /* standard pressure */
+	const float const_earth_gravity = 9.81f;
+
+	float posX = 0.0f;
+	float posY = 0.0f;
+	float posZ = 0.0f;
+
+	double lat_current;
+	double lon_current;
+	float alt_current;
+
+	float gps_origin_altitude = 0.0f;
+
+	/* Initialize filter */
+	kalman_dlqe2_initialize();
+	kalman_dlqe3_initialize();
+
+	/* declare and safely initialize all structs */
+	struct sensor_combined_s sensor;
+	memset(&sensor, 0, sizeof(sensor));
+	struct vehicle_attitude_s att;
+	memset(&att, 0, sizeof(att));
+	struct vehicle_status_s vehicle_status;
+	memset(&vehicle_status, 0, sizeof(vehicle_status)); /* make sure that baroINITdone = false */
+	struct vehicle_vicon_position_s vicon_pos;
+	memset(&vicon_pos, 0, sizeof(vicon_pos));
+	struct actuator_controls_effective_s act_eff;
+	memset(&act_eff, 0, sizeof(act_eff));
+	struct vehicle_gps_position_s gps;
+	memset(&gps, 0, sizeof(gps));
+	struct vehicle_local_position_s local_pos_est;
+	memset(&local_pos_est, 0, sizeof(local_pos_est));
+	struct vehicle_global_position_s global_pos_est;
+	memset(&global_pos_est, 0, sizeof(global_pos_est));
+
+	/* subscribe */
+	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	int sub_params = orb_subscribe(ORB_ID(parameter_update));
+	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	int vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
+	int actuator_eff_sub = orb_subscribe(ORB_ID(actuator_controls_effective_0));
+	int vehicle_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+
+	/* advertise */
+	orb_advert_t local_pos_est_pub = 0;
+	orb_advert_t global_pos_est_pub = 0;
+
+	struct position_estimator_mc_params pos1D_params;
+	struct position_estimator_mc_param_handles pos1D_param_handles;
+	/* initialize parameter handles */
+	parameters_init(&pos1D_param_handles);
+
+	bool flag_use_gps = false;
+	bool flag_use_baro = false;
+	bool flag_baro_initialized = false; /* in any case disable baroINITdone */
+	/* FIRST PARAMETER READ at START UP*/
+	struct parameter_update_s update;
+	orb_copy(ORB_ID(parameter_update), sub_params, &update); /* read from param to clear updated flag */
+	/* FIRST PARAMETER UPDATE */
+	parameters_update(&pos1D_param_handles, &pos1D_params);
+	flag_use_baro = pos1D_params.baro;
+	sigma = pos1D_params.sigma;
+	addNoise = pos1D_params.addNoise;
+	/* END FIRST PARAMETER UPDATE */
+
+	/* try to grab a vicon message - if it fails, go for GPS. */
+
+	/* make sure the next orb_check() can't return true unless we get a timely update */
+	orb_copy(ORB_ID(vehicle_vicon_position), vicon_pos_sub, &vicon_pos);
+	/* allow 200 ms for vicon to come in */
+	usleep(200000);
+	/* check if we got vicon */
+	bool update_check;
+	orb_check(vicon_pos_sub, &update_check);
+	/* if no update was available, use GPS */
+	flag_use_gps = !update_check;
+
+	if (flag_use_gps) {
+		mavlink_log_info(mavlink_fd, "[pos_est_mc] GPS locked");
+		/* wait until gps signal turns valid, only then can we initialize the projection */
+
+		// XXX magic number
+		float hdop_threshold_m = 4.0f;
+		float vdop_threshold_m = 8.0f;
+
+		/*
+		 * If horizontal dilution of precision (hdop / eph)
+		 * and vertical diluation of precision (vdop / epv)
+		 * are below a certain threshold (e.g. 4 m), AND
+		 * home position is not yet set AND the last GPS
+		 * GPS measurement is not older than two seconds AND
+		 * the system is currently not armed, set home
+		 * position to the current position.
+		 */
+
+		while (!(gps.fix_type == 3
+			&& (gps.eph_m < hdop_threshold_m)
+			&& (gps.epv_m < vdop_threshold_m)
+			&& (hrt_absolute_time() - gps.timestamp_position < 2000000))) {
+
+			struct pollfd fds1[2] = {
+					{ .fd = vehicle_gps_sub, .events = POLLIN },
+					{ .fd = sub_params,   .events = POLLIN },
+			};
+
+			/* wait for GPS updates, BUT READ VEHICLE STATUS (!)
+			 * this choice is critical, since the vehicle status might not
+			 * actually change, if this app is started after GPS lock was
+			 * aquired.
+			 */
+			if (poll(fds1, 2, 5000)) {
+				if (fds1[0].revents & POLLIN){
+					/* Read gps position */
+					orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
+				}
+				if (fds1[1].revents & POLLIN){
+					/* Read out parameters to check for an update there, e.g. useGPS variable */
+					/* read from param to clear updated flag */
+					struct parameter_update_s updated;
+					orb_copy(ORB_ID(parameter_update), sub_params, &updated);
+					/* update parameters */
+					parameters_update(&pos1D_param_handles, &pos1D_params);
+				}
+			}
+			static int printcounter = 0;
+			if (printcounter == 100) {
+				printcounter = 0;
+				warnx("[pos_est_mc] wait for GPS fix");
+			}
+			printcounter++;
+		}
+
+		/* get gps value for first initialization */
+		orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
+		lat_current = ((double)(gps.lat)) * 1e-7d;
+		lon_current = ((double)(gps.lon)) * 1e-7d;
+		alt_current = gps.alt * 1e-3f;
+		gps_origin_altitude = alt_current;
+		/* initialize coordinates */
+		map_projection_init(lat_current, lon_current);
+		/* publish global position messages only after first GPS message */
+		printf("[pos_est_mc] initialized projection with: lat: %.10f,  lon:%.10f\n", lat_current, lon_current);
+
+	} else {
+		mavlink_log_info(mavlink_fd, "[pos_est_mc] I'm NOT using GPS - I use VICON");
+		/* onboard calculated position estimations */
+	}
+	thread_running = true;
+
+	struct pollfd fds2[3] = {
+		{ .fd = vehicle_gps_sub,   .events = POLLIN },
+		{ .fd = vicon_pos_sub,   .events = POLLIN },
+		{ .fd = sub_params,   .events = POLLIN },
+	};
+
+	bool vicon_updated = false;
+	bool gps_updated = false;
+
+	/**< main_loop */
+	while (!thread_should_exit) {
+		int ret = poll(fds2, 3, 20);  //wait maximal this 20 ms = 50 Hz minimum rate
+		if (ret < 0) {
+			/* poll error */
+		} else {
+			if (fds2[2].revents & POLLIN){
+				/* new parameter */
+				/* read from param to clear updated flag */
+				struct parameter_update_s updated;
+				orb_copy(ORB_ID(parameter_update), sub_params, &updated);
+				/* update parameters */
+				parameters_update(&pos1D_param_handles, &pos1D_params);
+				flag_use_baro = pos1D_params.baro;
+				sigma = pos1D_params.sigma;
+				addNoise = pos1D_params.addNoise;
+			}
+			vicon_updated = false; /* default is no vicon_updated */
+			if (fds2[1].revents & POLLIN) {
+				/* new vicon position */
+				orb_copy(ORB_ID(vehicle_vicon_position), vicon_pos_sub, &vicon_pos);
+				posX = vicon_pos.x;
+				posY = vicon_pos.y;
+				posZ = vicon_pos.z;
+				vicon_updated = true; /* set flag for vicon update */
+			} /* end of poll call for vicon updates */
+			gps_updated = false;
+			if (fds2[0].revents & POLLIN) {
+				/* new GPS value */
+				orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
+				/* Project gps lat lon (Geographic coordinate system) to plane*/
+				map_projection_project(((double)(gps.lat)) * 1e-7d, ((double)(gps.lon)) * 1e-7d, &(z[0]), &(z[1]));
+				posX = z[0];
+				posY = z[1];
+				posZ = (float)(gps.alt * 1e-3f);
+				gps_updated = true;
+			}
+
+			/* Main estimator loop */
+			orb_copy(ORB_ID(actuator_controls_effective_0), actuator_eff_sub, &act_eff);
+			orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
+			orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
+			orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensor);
+			// barometric pressure estimation at start up
+			if (!flag_baro_initialized){
+				// mean calculation over several measurements
+				if (baro_loop_cnt<baro_loop_end) {
+					p0_Pa += (sensor.baro_pres_mbar*100);
+					baro_loop_cnt++;
+				} else {
+					p0_Pa /= (float)(baro_loop_cnt);
+					flag_baro_initialized = true;
+					char *baro_m_start = "barometer initialized with p0 = ";
+					char p0_char[15];
+					sprintf(p0_char, "%8.2f", (double)(p0_Pa/100));
+					char *baro_m_end = " mbar";
+					char str[80];
+					strcpy(str,baro_m_start);
+					strcat(str,p0_char);
+					strcat(str,baro_m_end);
+					mavlink_log_info(mavlink_fd, str);
+				}
+			}
+			if (flag_use_gps) {
+				/* initialize map projection with the last estimate (not at full rate) */
+				if (gps.fix_type > 2) {
+					/* x-y-position/velocity estimation in earth frame = gps frame */
+					kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_x_aposteriori_k,posX,gps_updated,0.0f,0.0f,x_x_aposteriori);
+					memcpy(x_x_aposteriori_k, x_x_aposteriori, sizeof(x_x_aposteriori));
+					kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_y_aposteriori_k,posY,gps_updated,0.0f,0.0f,x_y_aposteriori);
+					memcpy(x_y_aposteriori_k, x_y_aposteriori, sizeof(x_y_aposteriori));
+					/* z-position/velocity estimation in earth frame = vicon frame */
+					float z_est = 0.0f;
+					if (flag_baro_initialized && flag_use_baro) {
+						z_est = -p0_Pa*logf(p0_Pa/(sensor.baro_pres_mbar*100))/(rho0*const_earth_gravity);
+						K[0] = K_vicon_50Hz[0];
+						K[1] = K_vicon_50Hz[1];
+						K[2] = K_vicon_50Hz[2];
+						gps_updated = 1.0f; /* always enable the update, cause baro update = 200 Hz */
+					} else {
+						z_est = posZ;
+						K[0] = K_vicon_50Hz[0];
+						K[1] = K_vicon_50Hz[1];
+						K[2] = K_vicon_50Hz[2];
+					}
+
+					kalman_dlqe3(dT_const_50,K[0],K[1],K[2],x_z_aposteriori_k,z_est,gps_updated,0.0f,0.0f,x_z_aposteriori);
+					memcpy(x_z_aposteriori_k, x_z_aposteriori, sizeof(x_z_aposteriori));
+					local_pos_est.x = x_x_aposteriori_k[0];
+					local_pos_est.vx = x_x_aposteriori_k[1];
+					local_pos_est.y = x_y_aposteriori_k[0];
+					local_pos_est.vy = x_y_aposteriori_k[1];
+					local_pos_est.z = x_z_aposteriori_k[0];
+					local_pos_est.vz = x_z_aposteriori_k[1];
+					local_pos_est.timestamp = hrt_absolute_time();
+					if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))) {
+						/* publish local position estimate */
+						if (local_pos_est_pub > 0) {
+							orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est);
+						} else {
+							local_pos_est_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos_est);
+						}
+						/* publish on GPS updates */
+						if (gps_updated) {
+
+							double lat, lon;
+							float alt = z_est + gps_origin_altitude;
+
+							map_projection_reproject(local_pos_est.x, local_pos_est.y, &lat, &lon);
+
+							global_pos_est.lat = lat;
+							global_pos_est.lon = lon;
+							global_pos_est.alt = alt;
+
+							if (global_pos_est_pub > 0) {
+								orb_publish(ORB_ID(vehicle_global_position), global_pos_est_pub, &global_pos_est);
+							} else {
+								global_pos_est_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos_est);
+							}
+						}
+					}
+				}
+			} else {
+				/* x-y-position/velocity estimation in earth frame = vicon frame */
+				kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_x_aposteriori_k,posX,vicon_updated,addNoise,sigma,x_x_aposteriori);
+				memcpy(x_x_aposteriori_k, x_x_aposteriori, sizeof(x_x_aposteriori));
+				kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_y_aposteriori_k,posY,vicon_updated,addNoise,sigma,x_y_aposteriori);
+				memcpy(x_y_aposteriori_k, x_y_aposteriori, sizeof(x_y_aposteriori));
+				/* z-position/velocity estimation in earth frame = vicon frame */
+				float z_est = 0.0f;
+				float local_sigma = 0.0f;
+				if (flag_baro_initialized && flag_use_baro) {
+					z_est = -p0_Pa*logf(p0_Pa/(sensor.baro_pres_mbar*100.0f))/(rho0*const_earth_gravity);
+					K[0] = K_vicon_50Hz[0];
+					K[1] = K_vicon_50Hz[1];
+					K[2] = K_vicon_50Hz[2];
+					vicon_updated = 1; /* always enable the update, cause baro update = 200 Hz */
+					local_sigma = 0.0f; /* don't add noise on barometer in any case */
+				} else {
+					z_est = posZ;
+					K[0] = K_vicon_50Hz[0];
+					K[1] = K_vicon_50Hz[1];
+					K[2] = K_vicon_50Hz[2];
+					local_sigma = sigma;
+				}
+				kalman_dlqe3(dT_const_50,K[0],K[1],K[2],x_z_aposteriori_k,z_est,vicon_updated,addNoise,local_sigma,x_z_aposteriori);
+				memcpy(x_z_aposteriori_k, x_z_aposteriori, sizeof(x_z_aposteriori));
+				local_pos_est.x = x_x_aposteriori_k[0];
+				local_pos_est.vx = x_x_aposteriori_k[1];
+				local_pos_est.y = x_y_aposteriori_k[0];
+				local_pos_est.vy = x_y_aposteriori_k[1];
+				local_pos_est.z = x_z_aposteriori_k[0];
+				local_pos_est.vz = x_z_aposteriori_k[1];
+				local_pos_est.timestamp = hrt_absolute_time();
+				if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))){
+					orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est);
+				}
+			}
+		} /* end of poll return value check */
+	}
+
+	printf("[pos_est_mc] exiting.\n");
+	mavlink_log_info(mavlink_fd, "[pos_est_mc] exiting");
+	thread_running = false;
+	return 0;
+}
diff --git a/apps/position_estimator_mc/position_estimator_mc_params.c b/apps/position_estimator_mc/position_estimator_mc_params.c
new file mode 100755
index 000000000..72e5bc73b
--- /dev/null
+++ b/apps/position_estimator_mc/position_estimator_mc_params.c
@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: 	Damian Aregger <daregger@student.ethz.ch>
+ *   			Tobias Naegeli <naegelit@student.ethz.ch>
+* 				Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file position_estimator_mc_params.c
+ * 
+ * Parameters for position_estimator_mc
+ */
+
+#include "position_estimator_mc_params.h"
+
+/* Kalman Filter covariances */
+/* gps measurement noise standard deviation */
+PARAM_DEFINE_FLOAT(POS_EST_ADDN, 1.0f);
+PARAM_DEFINE_FLOAT(POS_EST_SIGMA, 0.0f);
+PARAM_DEFINE_FLOAT(POS_EST_R, 1.0f);
+PARAM_DEFINE_INT32(POS_EST_BARO, 0.0f);
+
+int parameters_init(struct position_estimator_mc_param_handles *h)
+{
+	h->addNoise	= param_find("POS_EST_ADDN");
+	h->sigma = param_find("POS_EST_SIGMA");
+	h->r = param_find("POS_EST_R");
+	h->baro_param_handle = param_find("POS_EST_BARO");
+	return OK;
+}
+
+int parameters_update(const struct position_estimator_mc_param_handles *h, struct position_estimator_mc_params *p)
+{
+	param_get(h->addNoise, &(p->addNoise));
+	param_get(h->sigma, &(p->sigma));
+	param_get(h->r, &(p->R));
+	param_get(h->baro_param_handle, &(p->baro));
+	return OK;
+}
diff --git a/apps/position_estimator_mc/position_estimator_mc_params.h b/apps/position_estimator_mc/position_estimator_mc_params.h
new file mode 100755
index 000000000..c4c95f7b4
--- /dev/null
+++ b/apps/position_estimator_mc/position_estimator_mc_params.h
@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: 	Damian Aregger <daregger@student.ethz.ch>
+ *   			Tobias Naegeli <naegelit@student.ethz.ch>
+ *				Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file position_estimator_mc_params.h
+ * 
+ * Parameters for Position Estimator
+ */
+
+#include <systemlib/param/param.h>
+
+struct position_estimator_mc_params {
+	float addNoise;
+	float sigma;
+	float R;
+	int baro; /* consider barometer */
+};
+
+struct position_estimator_mc_param_handles {
+	param_t addNoise;
+	param_t sigma;
+	param_t r;
+	param_t baro_param_handle;
+};
+
+/**
+ * Initialize all parameter handles and values
+ *
+ */
+int parameters_init(struct position_estimator_mc_param_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+int parameters_update(const struct position_estimator_mc_param_handles *h, struct position_estimator_mc_params *p);
diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig
index b5f35f514..68e7a49cf 100644
--- a/nuttx/configs/px4fmu/nsh/appconfig
+++ b/nuttx/configs/px4fmu/nsh/appconfig
@@ -97,7 +97,7 @@ ifneq ($(CONFIG_APM),y)
 CONFIGURED_APPS += ardrone_interface
 CONFIGURED_APPS += multirotor_att_control
 CONFIGURED_APPS += multirotor_pos_control
-#CONFIGURED_APPS += fixedwing_control
+CONFIGURED_APPS += position_estimator_mc
 CONFIGURED_APPS += fixedwing_att_control
 CONFIGURED_APPS += fixedwing_pos_control
 CONFIGURED_APPS += position_estimator