diff --git a/.DS_Store b/.DS_Store
new file mode 100644
index 0000000..612dcda
Binary files /dev/null and b/.DS_Store differ
diff --git a/.gitignore b/.gitignore
index a4be687..3f65655 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,11 +1,24 @@
 # gitignore file
-*
+fig/
+Latex/
+results/
+workspace/
+config/
+x86/
+doc/
+slprj/
+jackal/
 
-# exceptions (dir)
-!data/
-!doc/
-!Lib/
+# files
+*.asv
+*.save
+.gitignore
+*.autosave
+*.original
+*.slxc
+*.zip
+*.prj
+*.tmp
 
-#exceptions (files)
-!COPYING
-!README.md
\ No newline at end of file
+# exceptions
+!results/simulations/battery/report
diff --git a/BaseZoom.m b/BaseZoom.m
new file mode 100644
index 0000000..04778ed
--- /dev/null
+++ b/BaseZoom.m
@@ -0,0 +1,877 @@
+classdef BaseZoom < handle
+    %{
+
+        Interactive Magnification of Customized Regions.
+
+        Email: iqiukp@outlook.com
+    
+        -------------------------------------------------------------
+
+        Version 1.5.1, 5-FEB-2024
+            -- Support for zoom of a specified Axes object.
+            -- Support setting the number of connection lines.
+            -- Support for manual mode.
+            -- Fixed minor bugs.
+
+        Version 1.4, 30-MAY-2023
+            -- Added support for charts with two y-axes.
+            -- Customize parameters using json files.
+
+        Version 1.3.1, 24-JAN-2022
+            -- Fixed bugs when applied to logarithmic-scale coordinates. 
+
+        Version 1.3, 17-JAN-2022
+            -- Fixed minor bugs.
+            -- Added support for image class.
+
+        Version 1.2, 4-OCT-2021
+            -- Added support for interaction.
+
+        Version 1.1, 1-SEP-2021
+            -- Fixed minor bugs.
+            -- Added description of parameters.   
+
+        Version 1.0, 10-JUN-2021
+            -- Magnification of Customized Regions.
+
+        -------------------------------------------------------------
+        
+        BSD 3-Clause License
+        Copyright (c) 2024, Kepeng Qiu
+        All rights reserved.
+        
+    %}
+
+    % main  properties
+    properties
+        axesObject
+        subAxesPosition = [];
+        zoomAreaPosition = [];
+        zoomMode = 'interaction';
+        subFigure
+        mainAxes
+        subAxes
+        roi
+        zoomedArea
+        parameters
+        XAxis
+        YAxis
+        direction
+        % image-related properties
+        uPixels
+        vPixels
+        vuRatio
+        CData_
+        Colormap_
+        imageDim
+        imagePosition = [0.1, 0.1, 0.8, 0.6]
+        imageRectangleEdgePosition
+        imageArrow
+        % figure-related properties
+        mappingParams
+        figureRectangleEdgePosition
+        lineDirection
+        axesPosition
+        figureArrow
+        % others
+        drawFunc
+        axesClassName
+        isAxesDrawn = 'off'
+        isRectangleDrawn = 'off'
+        pauseTime = 0.2
+        textDisplay = 'on'
+    end
+
+    % dynamic properties
+    properties(Dependent)
+        XLimNew
+        YLimNew
+        affinePosition
+        dynamicPosition
+        newCData_
+        newCData
+        newCMap
+    end
+
+    methods
+
+        function this = BaseZoom(varargin)
+            % Check MATLAB version compatibility
+            if verLessThan('matlab', '9.5') % MATLAB 2018b version is 9.5
+                error('BaseZoom:IncompatibleVersion', 'Please use BaseZoom version 1.4 or below for your MATLAB version.');
+            end
+
+            switch nargin
+                case 0 % No input arguments
+                    this.axesObject = gca;
+                    this.zoomMode = 'interaction';
+                    this.textDisplay = 'on';
+
+                case 1 % One input argument
+                    this.axesObject = varargin{1};
+                    if ~isa(this.axesObject, 'matlab.graphics.axis.Axes')
+                        error('Input must be an Axes object.');
+                    end
+                    this.zoomMode = 'interaction';
+                    this.textDisplay = 'on';
+                    % Check if the object is an image or figure
+                    if ~isempty(imhandles(this.axesObject))
+                        this.axesClassName = 'image';
+                    else
+                        this.axesClassName = 'figure';
+                    end
+                case 2 % Two input arguments
+                    % Check if the first input argument is an Axes object or a position vector
+                    if isa(varargin{1}, 'matlab.graphics.axis.Axes')
+                        this.axesObject = varargin{1};
+                        if ~isempty(imhandles(this.axesObject))
+                            % The object is an image
+                            if ~all(isnumeric(varargin{2}) & numel(varargin{2}) == 4)
+                                error('The second input must be a numeric 4-element vector representing zoomAreaPosition.');
+                            end
+                            this.zoomAreaPosition = varargin{2};
+                            this.zoomMode = 'manual';
+                            this.textDisplay = 'off';
+                        else
+                            error('For two inputs, if the first input is an Axes object, it must be associated with an image.');
+                        end
+                    elseif all(isnumeric(varargin{1}) & numel(varargin{1}) == 4)
+                        this.subAxesPosition = varargin{1};
+                        this.zoomAreaPosition = varargin{2};
+                        this.axesObject = gca;
+                        if ~isempty(imhandles(this.axesObject))
+                            error('For two numeric inputs, the current Axes must not be associated with an image.');
+                        end
+                        this.zoomMode = 'manual';
+                        this.textDisplay = 'off';
+                    else
+                        error('For two inputs, the first input must be either an Axes object associated with an image or a numeric 4-element vector representing subAxesPosition.');
+                    end
+
+                case 3 % Three input arguments
+                    this.axesObject = varargin{1};
+                    this.subAxesPosition = varargin{2};
+                    this.zoomAreaPosition = varargin{3};
+                    if ~isa(this.axesObject, 'matlab.graphics.axis.Axes')
+                        error('The first input must be an Axes object.');
+                    end
+                    if ~isempty(imhandles(this.axesObject))
+                        error('For three inputs, the Axes object must not be associated with an image.');
+                    end
+                    if ~(all(isnumeric(this.subAxesPosition) & numel(this.subAxesPosition) == 4) && ...
+                            all(isnumeric(this.zoomAreaPosition) & numel(this.zoomAreaPosition) == 4))
+                        error('The second and third inputs must be numeric 4-element vectors representing subAxesPosition and zoomAreaPosition, respectively.');
+                    end
+                    this.zoomMode = 'manual';
+                    this.textDisplay = 'off';
+
+                otherwise
+                    error(['Invalid number of input arguments. ',...
+                        'For two inputs, provide either subAxesPosition and zoomAreaPosition, or an Axes object and zoomAreaPosition. ',...
+                        'For three inputs, provide an Axes object, subAxesPosition, and zoomAreaPosition.']);
+            end
+            this.initialize;
+            this.loadParameters;
+        end
+
+        function run(this)
+            % main steps
+            switch this.axesClassName
+                case 'image'
+                    this.addSubAxes;
+                    this.isAxesDrawn = 'off';
+                    this.displayZoomInstructions();
+                    this.addZoomedArea;
+                    this.isRectangleDrawn = 'off';
+                case 'figure'
+                    this.displaySubAxesInstructions();
+                    this.addSubAxes;
+                    this.isAxesDrawn = 'off';
+                    this.displayZoomInstructions();
+                    this.addZoomedArea;
+                    this.isRectangleDrawn = 'off';
+            end
+        end
+
+        function loadParameters(this)
+            fileName = 'parameters.json';
+            fid = fopen(fileName);
+            raw = fread(fid,inf);
+            str = char(raw');
+            fclose(fid);
+            this.parameters = jsondecode(str);
+            names_ = fieldnames(this.parameters);
+            for i = 1:length(names_)
+                if isfield(this.parameters.(names_{i}), 'Comments')
+                    this.parameters.(names_{i}) = rmfield(this.parameters.(names_{i}), 'Comments');
+                end
+            end
+        end
+
+        function initialize(this)
+            this.mainAxes = this.axesObject;
+            if size(imhandles(this.mainAxes),1) ~= 0
+                this.axesClassName = 'image';
+                this.CData_ = get(this.mainAxes.Children, 'CData');
+                this.Colormap_ = colormap(gca);
+                if size(this.Colormap_, 1) == 64
+                    this.Colormap_ = colormap(gcf);
+                end
+                [this.vPixels, this.uPixels, ~] = size(this.CData_);
+                this.vuRatio = this.vPixels/this.uPixels;
+                this.imageDim = length(size(this.CData_));
+            else
+                this.axesClassName = 'figure';
+            end
+
+            if strcmp(this.axesClassName, 'figure')
+                this.YAxis.direction = {'left', 'right'};
+                this.YAxis.number = length(this.mainAxes.YAxis);
+                this.XAxis.number = length(this.mainAxes.XAxis);
+                this.XAxis.scale = this.mainAxes.XScale;
+                this.direction = this.mainAxes.YAxisLocation;
+                switch this.YAxis.number
+                    case 1
+                        this.YAxis.(this.direction).scale = this.mainAxes.YScale;
+                    case 2
+                        for i = 1:2
+                            yyaxis(this.mainAxes, this.YAxis.direction{1, i});
+                            this.YAxis.(this.YAxis.direction{1, i}).scale = this.mainAxes.YScale;
+                            this.YAxis.scale{i} = this.mainAxes.YScale;
+                        end
+                        this.YAxis.scale = cell2mat(this.YAxis.scale);
+                        yyaxis(this.mainAxes, this.direction);
+                end
+            end
+        end
+
+        function addSubAxes(this)
+            switch this.axesClassName
+                case 'image'
+                    this.subFigure = figure;
+                    this.imagePosition(4) = this.imagePosition(3)*this.vuRatio;
+                    set(this.subFigure, 'Units', 'Normalized', 'OuterPosition', this.imagePosition);
+                    subplot(1, 2, 1, 'Parent', this.subFigure);
+                    image(this.CData_);
+                    this.mainAxes = gca;
+                    if this.imageDim == 2
+                        colormap(this.mainAxes, this.Colormap_);
+                    end
+                    axis off
+                    subplot(1, 2, 2, 'Parent', this.subFigure);
+                    image((ones(this.vPixels, this.uPixels)));
+                    this.subAxes = gca;
+                    colormap(this.subAxes, [240, 240, 240]/255);
+                    axis off
+                case 'figure' %
+                    if strcmp(this.zoomMode, 'interaction')
+                        this.roi = drawrectangle(this.mainAxes, 'Label', 'SubAxes');
+                        this.setTheme;
+                        this.creatSubAxes;
+                        set(gcf, 'WindowButtonDownFcn', {@this.clickEvents, 'subAxes'});
+                        addlistener(this.roi, 'MovingROI', @(source, event) ...
+                            this.allEvents(source, event, 'subAxes'));
+                        addlistener(this.roi, 'ROIMoved', @(source, event) ...
+                            this.allEvents(source, event, 'subAxes'));
+                        while strcmp(this.isAxesDrawn, 'off')
+                            pause(this.pauseTime);
+                        end
+                    else
+                        this.roi = drawrectangle(this.mainAxes,...
+                            'Position', this.subAxesPosition, 'Label', 'SubAxes');
+                        this.creatSubAxes;
+                        delete(this.roi);
+                        set(this.subAxes, 'Visible', 'on')
+                        this.isAxesDrawn = 'on';
+                    end
+                    % this.subAxes.Color = this.mainAxes.Color;
+            end
+        end
+
+        function addZoomedArea(this)
+            switch this.axesClassName
+                case 'image'
+                    if strcmp(this.zoomMode, 'interaction')
+                        this.roi = drawrectangle(this.mainAxes, 'Label', 'ZoomArea');
+                        this.setTheme;
+                        this.creatSubAxes;
+                        if strcmp(this.parameters.subAxes.Box, 'on')
+                            this.connectAxesAndBox;
+                        end
+                        set(gcf, 'WindowButtonDownFcn', {@this.clickEvents, 'zoomedArea'});
+                        addlistener(this.roi, 'MovingROI', @(source, event) ...
+                            this.allEvents(source, event, 'zoomedArea'));
+                        addlistener(this.roi, 'ROIMoved', @(source, event) ...
+                            this.allEvents(source, event, 'zoomedArea'));
+                        while strcmp(this.isRectangleDrawn, 'off')
+                            pause(this.pauseTime);
+                        end
+                    else
+
+                        this.roi = drawrectangle(this.mainAxes,...
+                            'Position', this.zoomAreaPosition, 'Label', 'zoomArea');
+                        this.setTheme;
+                        if strcmp(this.parameters.subAxes.Box, 'on')
+                            this.connectAxesAndBox;
+                        end
+                        this.creatSubAxes;
+                        this.isRectangleDrawn = 'on';
+                        this.createRectangle;
+                        delete(this.roi);
+                    end
+                    for iArrow = 1:length(this.imageArrow)
+                        this.imageArrow{iArrow}.Tag = 'ZoomPlot';
+                    end
+
+                case 'figure' %
+                    if strcmp(this.zoomMode, 'interaction')
+                        this.roi = drawrectangle(this.mainAxes, 'Label', 'zoomArea');
+                        this.setTheme;
+                        if strcmp(this.parameters.subAxes.Box, 'on')
+                            this.connectAxesAndBox;
+                        end
+                        this.setSubAxesLim;
+                        set(gcf, 'WindowButtonDownFcn', {@this.clickEvents, 'zoomedArea'});
+                        addlistener(this.roi, 'MovingROI', @(source, event) ...
+                            this.allEvents(source, event, 'zoomedArea'));
+                        addlistener(this.roi, 'ROIMoved', @(source, event) ...
+                            this.allEvents(source, event, 'zoomedArea'));
+                        while strcmp(this.isRectangleDrawn, 'off')
+                            pause(this.pauseTime);
+                        end
+                    else
+                        this.roi = drawrectangle(this.mainAxes,...
+                            'Position', this.zoomAreaPosition, 'Label', 'zoomArea');
+                        this.setTheme;
+                        if strcmp(this.parameters.subAxes.Box, 'on')
+                            this.connectAxesAndBox;
+                        end
+                        this.setSubAxesLim;
+                        this.isRectangleDrawn = 'on';
+                        this.createRectangle;
+                        delete(this.roi);
+                    end
+                    for iArrow = 1:length(this.figureArrow)
+                        this.figureArrow{iArrow}.Tag = 'ZoomPlot';
+                    end
+            end
+        end
+
+        function allEvents(this,  ~, ~, mode)
+            switch mode
+                case 'subAxes'
+                    if strcmp(this.textDisplay, 'on')
+                        fprintf('adjust the sub axes...\n');
+                    end
+                    delete(this.subAxes);
+                    this.creatSubAxes;
+                    this.subAxes.Color = this.parameters.subAxes.Color;
+                case 'zoomedArea'
+                    if strcmp(this.textDisplay, 'on')
+                        fprintf('adjust the zoomed area...\n')
+                    end
+                    delete(findall(gcf, 'Tag', 'ZoomPlot_'))
+                    if strcmp(this.parameters.subAxes.Box, 'on')
+                        this.connectAxesAndBox;
+                    end
+                    switch this.axesClassName
+                        case 'image' %
+                            this.creatSubAxes;
+                        case 'figure' %
+                            this.setSubAxesLim;
+                    end
+            end
+        end
+
+        function clickEvents(this, ~, ~, mode)
+            switch mode
+                case 'subAxes'
+                    switch get(gcf, 'SelectionType')
+                        case 'alt'
+                            this.isAxesDrawn = 'on';
+                            set(this.subAxes, 'Visible', 'on');
+                            set(gcf, 'WindowButtonDownFcn', []);
+                            if strcmp(this.textDisplay, 'on')
+                                fprintf('Complete the adjustment of the sub axes.\n\n');
+                            end
+                            delete(this.roi);
+                            this.subAxes.Color = this.parameters.subAxes.Color;
+
+                        case 'normal'
+                            this.isAxesDrawn = 'off';
+                            if strcmp(this.textDisplay, 'on')
+                                fprintf('Right-click to stop adjusting.\n');
+                            end
+                            this.subAxes.Color = this.parameters.subAxes.Color;
+
+                        otherwise
+                            this.isAxesDrawn = 'off';
+                            if strcmp(this.textDisplay, 'on')
+                                fprintf('Right-click to stop adjusting.\n');
+                            end
+                            this.subAxes.Color = this.parameters.subAxes.Color;
+                    end
+
+                case 'zoomedArea'
+                    switch get(gcf, 'SelectionType')
+                        case 'alt'
+                            this.isRectangleDrawn = 'on';
+                            this.createRectangle;
+                            set(gcf, 'WindowButtonDownFcn', []);
+                            delete(this.roi);
+                            if strcmp(this.textDisplay, 'on')
+                                fprintf('Complete the adjustment of the zoomed area.\n\n');
+                            end
+                        case 'normal'
+                            this.isRectangleDrawn = 'off';
+                            if strcmp(this.textDisplay, 'on')
+                                fprintf('Right-click to stop adjusting.\n');
+                            end
+                        otherwise
+                            this.isRectangleDrawn = 'off';
+                            if strcmp(this.textDisplay, 'on')
+                                fprintf('Right-click to stop adjusting.\n');
+                            end
+                    end
+            end
+        end
+
+        function creatSubAxes(this)
+            switch this.axesClassName
+                case 'image'
+                    set(this.subAxes.Children, 'CData', this.newCData);
+                    if this.imageDim == 2
+                        colormap(this.subAxes, this.newCMap);
+                    end
+                case 'figure'
+                    if this.YAxis.number == 1
+                        this.subAxes = axes('Position', this.affinePosition,...
+                            'XScale', this.XAxis.scale,...
+                            'YScale', this.YAxis.(this.direction).scale,...
+                            'parent', get(this.mainAxes, 'Parent'));
+                        mainChildren = this.getMainChildren;
+                        copyobj(mainChildren, this.subAxes);
+                        this.subAxes.XLim = this.mainAxes.XLim;
+                        hold(this.subAxes, 'on');
+                        set(this.subAxes, this.parameters.subAxes);
+                        set(this.subAxes, 'Visible', 'off');
+                    end
+                    if this.YAxis.number == 2
+                        diret_ = this.YAxis.direction;
+                        this.subAxes = axes('Position', this.affinePosition, 'parent', get(this.mainAxes, 'Parent'));
+                        for i = 1:2
+                            yyaxis(this.subAxes, diret_{i});
+                            yyaxis(this.mainAxes, diret_{i});
+                            set(this.subAxes, 'XScale', this.mainAxes.XScale,...
+                                'YScale', this.mainAxes.YScale)
+                            mainChildren = this.getMainChildren;
+                            copyobj(mainChildren, this.subAxes);
+                            this.subAxes.XLim = this.mainAxes.XLim;
+                            YLim.(diret_{i}) = this.subAxes.YLim;
+                        end
+                        yyaxis(this.mainAxes, this.direction);
+                        switch this.YAxis.scale
+                            case 'linearlinear'
+                                Y_from = YLim.(this.direction);
+                                Y_to = YLim.(cell2mat(setdiff(diret_, this.direction)));
+                            case 'linearlog'
+                                Y_from = YLim.(this.direction);
+                                Y_to = log10(YLim.(cell2mat(setdiff(diret_, this.direction))));
+                            case 'loglinear'
+                                Y_from = log10(YLim.(this.direction));
+                                Y_to = YLim.(cell2mat(setdiff(diret_, this.direction)));
+                            case 'loglog'
+                                Y_from = log10(YLim.(this.direction));
+                                Y_to = log10(YLim.(cell2mat(setdiff(diret_, this.direction))));
+                        end
+                        this.YAxis.K = (Y_to(2)-Y_to(1))/(Y_from(2)-Y_from(1));
+                        this.YAxis.b = Y_to(1)-Y_from(1)*this.YAxis.K;
+                        hold(this.subAxes, 'on');
+                        set(this.subAxes, this.parameters.subAxes);
+                        set(this.subAxes, 'Visible', 'off');
+                    end
+            end
+        end
+
+        function createRectangle(this)
+            % Determine rectangle position based on axes class
+            switch this.axesClassName
+                case 'image'
+                    position = this.imageRectangleEdgePosition;
+                case 'figure'
+                    position = this.affinePosition;
+            end
+
+            % Create the rectangle annotation with common properties
+            this.zoomedArea = annotation('rectangle', position, ...
+                'Color', this.parameters.zoomedArea.Color, ...
+                'FaceColor', this.parameters.zoomedArea.FaceColor, ...
+                'FaceAlpha', this.parameters.zoomedArea.FaceAlpha, ...
+                'LineStyle', this.parameters.zoomedArea.LineStyle, ...
+                'LineWidth', this.parameters.zoomedArea.LineWidth);
+        end
+
+        function mappingParams = computeMappingParams(this)
+            % Compute the mapping parameters for both axes
+            [map_k_x, map_b_x] = this.computeAxisMappingParams(this.XAxis.scale, ...
+                this.mainAxes.XLim, ...
+                this.mainAxes.Position(1), ...
+                this.mainAxes.Position(3));
+            [map_k_y, map_b_y] = this.computeAxisMappingParams(this.YAxis.(this.direction).scale, ...
+                this.mainAxes.YLim, ...
+                this.mainAxes.Position(2), ...
+                this.mainAxes.Position(4));
+            % Construct the mapping parameters matrix
+            mappingParams = [map_k_x, map_b_x; map_k_y, map_b_y];
+        end
+
+        function [map_k, map_b] = computeAxisMappingParams(~, scale, axesLim, pos, size)
+            % Compute mapping parameters based on the scale (linear or log)
+            switch scale
+                case 'linear'
+                    rangeLim = axesLim(2) - axesLim(1);
+                case 'log'
+                    rangeLim = log10(axesLim(2)) - log10(axesLim(1));
+                otherwise
+                    error('BaseZoom:InvalidScale', 'Unsupported axis scale.');
+            end
+            % Compute the scale factor and offset for mapping
+            map_k = rangeLim / size;
+            switch scale
+                case 'linear'
+                    map_b = axesLim(1) - pos * map_k;
+                case 'log'
+                    map_b = log10(axesLim(1)) - pos * map_k;
+            end
+        end
+
+        function connectAxesAndBox(this)
+            % insert lines between the inserted axes and rectangle
+
+            %   Rectangle        subAxes
+            %    2----1          2----1
+            %    3----4          3----4
+            switch this.axesClassName
+                case 'image' %
+                    uPixelsAll = this.uPixels/this.mainAxes.Position(3);
+                    vPixelsAll = this.vPixels/this.mainAxes.Position(4);
+                    Position_ = this.roi.Position;
+                    this.imageRectangleEdgePosition(1) = Position_(1)/uPixelsAll+this.mainAxes.Position(1);
+                    this.imageRectangleEdgePosition(2) = (this.vPixels-Position_(2)-Position_(4))/...
+                        vPixelsAll+this.subAxes.Position(2);
+                    this.imageRectangleEdgePosition(3) = Position_(3)/uPixelsAll;
+                    this.imageRectangleEdgePosition(4) = Position_(4)/vPixelsAll;
+                    % annotation position 1
+                    annotationPosX_1(1) = this.imageRectangleEdgePosition(1)+this.imageRectangleEdgePosition(3);
+                    annotationPosX_1(2) = this.subAxes.Position(1);
+                    annotationPosY_1(1) = this.imageRectangleEdgePosition(2);
+                    annotationPosY_1(2) = this.subAxes.Position(2);
+                    this.imageArrow{1} = annotation(gcf, 'doublearrow',...
+                        annotationPosX_1, annotationPosY_1,...
+                        'Color', this.parameters.connection.LineColor,...
+                        'LineWidth', this.parameters.connection.LineWidth,...
+                        'LineStyle', this.parameters.connection.LineStyle,...
+                        'Head1Style', this.parameters.connection.StartHeadStyle,...
+                        'Head1Length', this.parameters.connection.StartHeadLength,...
+                        'Head1Width', this.parameters.connection.StartHeadWidth,...
+                        'Head2Style', this.parameters.connection.EndHeadStyle,...
+                        'Head2Length', this.parameters.connection.EndHeadLength,...
+                        'Head2Width', this.parameters.connection.EndHeadWidth,...
+                        'Tag', 'ZoomPlot_');
+                    % annotation position 2
+                    annotationPosX_2(1) = this.imageRectangleEdgePosition(1)+this.imageRectangleEdgePosition(3);
+                    annotationPosX_2(2) = this.subAxes.Position(1);
+                    annotationPosY_2(1) = this.imageRectangleEdgePosition(2)+this.imageRectangleEdgePosition(4);
+                    annotationPosY_2(2) = this.subAxes.Position(2)+this.subAxes.Position(4);
+                    this.imageArrow{2} = annotation(gcf, 'doublearrow',...
+                        annotationPosX_2, annotationPosY_2,...
+                        'Color', this.parameters.connection.LineColor,...
+                        'LineWidth', this.parameters.connection.LineWidth,...
+                        'LineStyle', this.parameters.connection.LineStyle,...
+                        'Head1Style', this.parameters.connection.StartHeadStyle,...
+                        'Head1Length', this.parameters.connection.StartHeadLength,...
+                        'Head1Width', this.parameters.connection.StartHeadWidth,...
+                        'Head2Style', this.parameters.connection.EndHeadStyle,...
+                        'Head2Length', this.parameters.connection.EndHeadLength,...
+                        'Head2Width', this.parameters.connection.EndHeadWidth,...
+                        'Tag', 'ZoomPlot_');
+                case 'figure'
+                    % real coordinates of the inserted rectangle and axes
+                    this.getAxesAndBoxPosition;
+                    % get the line direction
+                    this.getLineDirection;
+                    % insert lines
+                    % numLine = size(this.lineDirection, 1);
+                    switch this.parameters.connection.LineNumber
+                        case 0
+
+                        case 1
+                            lineDirection_ = this.lineDirection(end, :);
+                        case 2
+                            lineDirection_ = this.lineDirection(1:2, :);
+                        otherwise
+                            error('The LineNumber must be 0 or 1 or 2.')
+                    end
+
+                    for i = 1:this.parameters.connection.LineNumber
+                        tmp1 = [this.figureRectangleEdgePosition(lineDirection_(i, 1), 1),...
+                            this.figureRectangleEdgePosition(lineDirection_(i, 1), 2)];
+                        tmp2 = [this.axesPosition(lineDirection_(i, 2), 1),...
+                            this.axesPosition(lineDirection_(i, 2), 2)];
+                        pos1 = this.transformCoordinate(tmp1, 'a2n');
+                        pos2 = this.transformCoordinate(tmp2, 'a2n');
+                        this.figureArrow{i} = annotation(gcf, 'doublearrow',...
+                            [pos1(1, 1), pos2(1, 1)], [pos1(1, 2), pos2(1, 2)],...
+                            'Color', this.parameters.connection.LineColor,...
+                            'LineWidth', this.parameters.connection.LineWidth,...
+                            'LineStyle', this.parameters.connection.LineStyle,...
+                            'Head1Style', this.parameters.connection.StartHeadStyle,...
+                            'Head1Length', this.parameters.connection.StartHeadLength,...
+                            'Head1Width', this.parameters.connection.StartHeadWidth,...
+                            'Head2Style', this.parameters.connection.EndHeadStyle,...
+                            'Head2Length', this.parameters.connection.EndHeadLength,...
+                            'Head2Width', this.parameters.connection.EndHeadWidth,...
+                            'Tag', 'ZoomPlot_');
+                    end
+            end
+        end
+
+        function getAxesAndBoxPosition(this)
+            % real coordinates of the inserted rectangle
+            box1_1 = [this.XLimNew(1, 2), this.YLimNew(1, 2)];
+            box1_2 = [this.XLimNew(1, 1), this.YLimNew(1, 2)];
+            box1_3 = [this.XLimNew(1, 1), this.YLimNew(1, 1)];
+            box1_4 = [this.XLimNew(1, 2), this.YLimNew(1, 1)];
+            box1 = [box1_1; box1_2; box1_3; box1_4];
+            % real coordinates of the inserted axes
+            tmp1 = [this.subAxes.Position(1)+this.subAxes.Position(3),...
+                this.subAxes.Position(2)+this.subAxes.Position(4)];
+            box2_1 = this.transformCoordinate(tmp1, 'n2a');
+            tmp2 = [this.subAxes.Position(1),...
+                this.subAxes.Position(2)+this.subAxes.Position(4)];
+            box2_2 = this.transformCoordinate(tmp2, 'n2a');
+            tmp3 = [this.subAxes.Position(1), this.subAxes.Position(2)];
+            box2_3 = this.transformCoordinate(tmp3, 'n2a');
+            tmp4 = [this.subAxes.Position(1)+this.subAxes.Position(3),...
+                this.subAxes.Position(2)];
+            box2_4 = this.transformCoordinate(tmp4, 'n2a');
+            box2 = [box2_1; box2_2; box2_3; box2_4];
+            this.figureRectangleEdgePosition = box1;
+            this.axesPosition = box2;
+        end
+
+        function getLineDirection(this)
+            % get the line direction
+            % left-upper
+            if (this.figureRectangleEdgePosition(4, 1) < this.axesPosition(1, 1) &&...
+                    this.figureRectangleEdgePosition(4, 2) > this.axesPosition(2, 2))
+                this.lineDirection = [3, 3; 1, 1; 4, 2];
+            end
+            % middle-upper
+            if (this.figureRectangleEdgePosition(4, 1) > this.axesPosition(2, 1) &&...
+                    this.figureRectangleEdgePosition(4, 2) > this.axesPosition(2, 2)) &&...
+                    this.figureRectangleEdgePosition(3, 1) < this.axesPosition(1, 1)
+                this.lineDirection = [4, 1; 3, 2; 4 ,1];
+            end
+            % right-upper
+            if (this.figureRectangleEdgePosition(3, 1) > this.axesPosition(1, 1) &&...
+                    this.figureRectangleEdgePosition(3, 2) > this.axesPosition(1, 2))
+                this.lineDirection = [2, 2; 4, 4; 3, 1];
+            end
+            % right-middle
+            if (this.figureRectangleEdgePosition(3, 1) > this.axesPosition(1, 1) &&...
+                    this.figureRectangleEdgePosition(3, 2) < this.axesPosition(1, 2)) &&...
+                    this.figureRectangleEdgePosition(2, 2) > this.axesPosition(4, 2)
+                this.lineDirection = [2, 1; 3, 4; 3, 1];
+            end
+            % right-down
+            if (this.figureRectangleEdgePosition(2, 1) > this.axesPosition(4, 1) &&...
+                    this.figureRectangleEdgePosition(2, 2) < this.axesPosition(4, 2))
+                this.lineDirection = [1, 1; 3, 3; 4, 2];
+            end
+            % down-middle
+            if (this.figureRectangleEdgePosition(1, 1) > this.axesPosition(3, 1) &&...
+                    this.figureRectangleEdgePosition(1, 2) < this.axesPosition(3, 2) &&...
+                    this.figureRectangleEdgePosition(2, 1) < this.axesPosition(4, 1))
+                this.lineDirection = [2, 3; 1, 4; 2, 4];
+            end
+            % left-down
+            if (this.figureRectangleEdgePosition(1, 1) < this.axesPosition(3, 1) &&...
+                    this.figureRectangleEdgePosition(1, 2) < this.axesPosition(3, 2))
+                this.lineDirection = [2, 2; 4, 4; 3, 1];
+            end
+            % left-middle
+            if (this.figureRectangleEdgePosition(4, 1) <this.axesPosition(2, 1) &&...
+                    this.figureRectangleEdgePosition(4, 2) < this.axesPosition(2, 2)) &&...
+                    this.figureRectangleEdgePosition(1, 2) > this.axesPosition(3, 2)
+                this.lineDirection = [1, 2; 4, 3; 1, 3];
+            end
+        end
+
+        function setSubAxesLim(this)
+            switch this.YAxis.number
+                case 1
+                    set(this.subAxes, 'XLim', this.XLimNew, 'YLim', this.YLimNew);
+                case 2
+                    yyaxis(this.subAxes, this.direction);
+                    set(this.subAxes, 'XLim', this.XLimNew, 'YLim', this.YLimNew);
+                    yyaxis(this.subAxes, 'left');
+                    switch this.YAxis.scale
+                        case 'linearlinear'
+                            Y_from = this.YLimNew;
+                            Y_to(1) = Y_from(1)*this.YAxis.K+this.YAxis.b;
+                            Y_to(2) = Y_from(2)*this.YAxis.K+this.YAxis.b;
+                        case 'linearlog'
+                            Y_from = this.YLimNew;
+                            Y_to(1) = 10.^(Y_from(1)*this.YAxis.K+this.YAxis.b);
+                            Y_to(2) = 10.^(Y_from(2)*this.YAxis.K+this.YAxis.b);
+                        case 'loglinear'
+                            Y_from = log10(this.YLimNew);
+                            Y_to(1) = Y_from(1)*this.YAxis.K+this.YAxis.b;
+                            Y_to(2) = Y_from(2)*this.YAxis.K+this.YAxis.b;
+                        case 'loglog'
+                            Y_from = log10(this.YLimNew);
+                            Y_to(1) = 10.^(Y_from(1)*this.YAxis.K+this.YAxis.b);
+                            Y_to(2) = 10.^(Y_from(2)*this.YAxis.K+this.YAxis.b);
+                    end
+                    set(this.subAxes, 'XLim', this.XLimNew,'YLim', Y_to);
+            end
+        end
+
+        function mainChildren = getMainChildren(this)
+            children_ = get(this.mainAxes, 'children');
+            numChildren_ = 1:length(children_);
+            for ii = 1:length(children_)
+                if strcmp(children_(ii, 1).Type, 'images.roi.rectangle') ||...
+                        strcmp(children_(ii, 1).Type, 'hggroup')
+                    numChildren_(ii) = [];
+                end
+            end
+            mainChildren = children_(numChildren_);
+        end
+
+        function setTheme(this)
+            % set the theme of the dynamic rectangle
+            try
+                this.roi.MarkerSize = this.parameters.dynamicRect.MarkerSize;
+            catch
+            end
+            this.roi.Color = this.parameters.dynamicRect.FaceColor;
+            this.roi.FaceAlpha = this.parameters.dynamicRect.FaceAspect;
+            this.roi.LineWidth = this.parameters.dynamicRect.LineWidth;
+        end
+
+        function coordinate = transformCoordinate(this, coordinate, type)
+            % coordinate transformation
+            switch type
+                % absolute coordinates to normalized coordinates
+                case 'a2n'
+                    switch this.XAxis.scale
+                        case 'linear'
+                            coordinate(1, 1) = (coordinate(1, 1)-this.mappingParams(1, 2))...
+                                /this.mappingParams(1, 1);
+                        case 'log'
+                            coordinate(1, 1) = (log10(coordinate(1, 1))-this.mappingParams(1, 2))...
+                                /this.mappingParams(1, 1);
+                    end
+
+                    switch this.YAxis.(this.direction).scale
+                        case 'linear'
+                            coordinate(1, 2) = (coordinate(1, 2)-this.mappingParams(2, 2))...
+                                /this.mappingParams(2, 1);
+                        case 'log'
+                            coordinate(1, 2) = (log10(coordinate(1, 2))-this.mappingParams(2, 2))...
+                                /this.mappingParams(2, 1);
+                    end
+                    % normalized coordinates to absolute coordinates
+                case 'n2a'
+                    switch this.XAxis.scale
+                        case 'linear'
+                            coordinate(1, 1) = coordinate(1, 1)*this.mappingParams(1, 1)...
+                                +this.mappingParams(1, 2);
+                        case 'log'
+                            coordinate(1, 1) = 10^(coordinate(1, 1)*this.mappingParams(1, 1)...
+                                +this.mappingParams(1, 2));
+                    end
+                    switch this.YAxis.(this.direction).scale
+                        case 'linear'
+                            coordinate(1, 2) = coordinate(1, 2)*this.mappingParams(2, 1)...
+                                +this.mappingParams(2, 2);
+                        case 'log'
+                            coordinate(1, 2) = 10^(coordinate(1, 2)*this.mappingParams(2, 1)...
+                                +this.mappingParams(2, 2));
+                    end
+            end
+        end
+
+        function throwError(~, message)
+            error('BaseZoom:InvalidInput', message);
+        end
+
+        function displaySubAxesInstructions(this)
+            if strcmp(this.textDisplay, 'on')
+                fprintf('Use the left mouse button to draw a rectangle.\n');
+                fprintf('for the sub axes...\n');
+            end
+        end
+
+        function displayZoomInstructions(this)
+            if strcmp(this.textDisplay, 'on')
+                fprintf('Use the left mouse button to draw a rectangle.\n');
+                fprintf('for the zoomed area...\n');
+            end
+        end
+
+        % dependent properties
+        function dynamicPosition = get.dynamicPosition(this)
+            dynamicPosition = this.roi.Position;
+        end
+
+        % dependent properties
+        function XLimNew = get.XLimNew(this)
+            XLimNew = [this.dynamicPosition(1), this.dynamicPosition(1)+this.dynamicPosition(3)];
+        end
+
+        % dependent properties
+        function YLimNew = get.YLimNew(this)
+            YLimNew = [this.dynamicPosition(2), this.dynamicPosition(2)+this.dynamicPosition(4)];
+        end
+
+        % dependent properties
+        function affinePosition = get.affinePosition(this)
+            this.mappingParams = this.computeMappingParams;
+            tmp1 = this.transformCoordinate([this.XLimNew(1, 1), this.YLimNew(1, 1)], 'a2n');
+            tmp2 = this.transformCoordinate([this.XLimNew(1, 2), this.YLimNew(1, 2)], 'a2n');
+            affinePosition(1, 1) = tmp1(1, 1);
+            affinePosition(1, 2) = tmp1(1, 2);
+            affinePosition(1, 3) = tmp2(1, 1)-tmp1(1, 1);
+            affinePosition(1, 4) = tmp2(1, 2)-tmp1(1, 2);
+        end
+
+        % dependent properties
+        function newCData_ = get.newCData_(this)
+            newCData_ = imcrop(this.CData_,this.Colormap_, this.roi.Position);
+        end
+
+        % dependent properties
+        function newCData = get.newCData(this)
+            switch this.imageDim
+                case 2
+                    [newCData, ~] = imresize(this.newCData_, this.Colormap_, [this.vPixels, this.uPixels]);
+                    %  [~, newCMap] = imresize(this.newCData_, this.newCMap_, [this.vPixels, this.uPixels]);
+                case 3
+                    newCData = imresize(this.newCData_, [this.vPixels, this.uPixels]);
+            end
+        end
+
+        % dependent properties
+        function newCMap = get.newCMap(this)
+            switch this.imageDim
+                case 2
+                    [~, newCMap] = imresize(this.newCData_, this.Colormap_, [this.vPixels, this.uPixels]);
+                case 3
+                    newCMap=[];
+            end
+        end
+    end
+end
\ No newline at end of file
diff --git a/Lib/.DS_Store b/Lib/.DS_Store
new file mode 100644
index 0000000..123d335
Binary files /dev/null and b/Lib/.DS_Store differ
diff --git a/Lib/control/control.m b/Lib/control/control.m
index fae3aac..c48f6a8 100644
--- a/Lib/control/control.m
+++ b/Lib/control/control.m
@@ -1,26 +1,91 @@
-%% CONTROL
-% file: control.m
-% author: Federico Oliva
-% date: 10/01/2022
-% description: this function implements the control law
-% INPUT:
-% t: time instant
-% drive: error variable
-% params: structure with all the necessary parameters 
-% OUTPUT:
-% u: control variable
-function u = control(t,drive,params)
-
-    % check if the input is enabled
+function u = control(t,drive,params,obs)
+
+    % init input
+    u = zeros(params.dim_input,length(t));
+
     if params.input_enable
+
+        % traj
+        traj = obs.init.traj;
+
+        % PWM 3 levels
+
+        % 2nd order system
+%         u(1,:) = -2*params.rhox(1)*params.wnx(obs.init.traj)*drive(2) -params.wnx(obs.init.traj)^2*drive(1);
+%         u(2,:) = -2*params.rhoy(1)*params.wny(obs.init.traj)*drive(6) -params.wny(obs.init.traj)^2*drive(5);
+        
+        % sum of sines (position)
+%         target(1,:) = exp(-params.rhox.*t).*(params.Ax(1)*sin(params.wnx(1).*t + params.phi(1)) + params.Ax(2)*sin(params.wnx(2).*t + params.phi(1)));
+%         target(2,:) = exp(-params.rhoy.*t).*(params.Ay(1)*sin(params.wny(1).*t + params.phi(2)) + params.Ay(2)*sin(params.wny(2).*t + params.phi(2)));
+%         u(1,:) = params.Ku(1)*(target(1,:)-drive(1,:));
+%         u(2,:) = params.Ku(2)*(target(2,:)-drive(6,:));
         
-    % !REMARK!
-    % simply uncomment or add the law you need, according to params
 
-    %%%% control law - battery %%%%
-    u = [sin(t); zeros(size(t))];
+        % patrolling on x-y
+        T = t(1);
+        vx = 0;
+        vy = 0;
+        if mod(T,params.freq_u) < 0.25*params.freq_u
+            vx = params.amp_ux;
+        elseif mod(T,params.freq_u) < 0.5*params.freq_u
+            vy = params.amp_uy;
+        elseif mod(T,params.freq_u) < 0.75*params.freq_u
+            vx = -params.amp_ux;
+        else
+            vy = -params.amp_uy;
+        end        
+        u(1,:) = params.Ku(1)*(vx-drive(params.pos_v(1)));
+        u(2,:) = params.Ku(2)*(vy-drive(params.pos_v(2)));
         
-    else
-        u = zeros(params.dim_input,1);
+        % compute the time index
+        for i=1:length(t)
+            tdiff = obs.setup.time-t(i);   
+            pos(i) = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+            pos(i) = max(1,pos(i));        
+        end  
+        ind = pos(1);
+
+        % hills on z
+        p_now = drive(params.pos_p(1:2));
+        p_est = zeros(1,2);
+        p_grid = [params.X_gauss(1,:); params.Y_gauss(:,1)'];
+        for i=1:2
+            pdiff = p_grid(i,:)-p_now(i);   
+            p_est(i) = find(abs(pdiff) == min(abs(pdiff)),1,'first');                
+        end
+        z_des = params.G_gauss(p_est(2),p_est(1));
+        z_now = drive(params.pos_p(3));
+        zdot_now = drive(params.pos_v(3));
+
+        obs.init.params.z_des_story(:,ind) = z_des;
+        obs.init.params.z_now_story(:,ind) = z_now;
+
+        % error        
+        e = (z_des-z_now);
+        obs.init.params.err(traj).val(:,ind) = e;
+
+        % derivative        
+        [edot, obs.init.params.err_der_buffer, obs.init.params.err_der_counter(traj).val] = PseudoDer(params.Ts,...
+            obs.init.params.err(traj).val(:,max(1,ind-1)),params.wlen_err,...
+            params.buflen_err,params.dim_err,0,0,obs,obs.init.params.err_der_buffer,obs.init.params.err_der_counter(traj).val);
+        obs.init.params.err_der(traj).val(:,ind) = edot;
+
+        % integral
+        obs.init.params.err_int(traj).val(:,ind) = obs.init.params.err_int(traj).val(:,max(1,ind-1)) + e*params.Ts;
+        eint = obs.init.params.err_int(traj).val(:,ind);
+
+        % control
+        u(3,:) = -params.Kz(1)*-e -params.Kz(2)*zdot_now + params.Kff*[e edot eint]';
+
+        % ony for testing
+        u(4,:) = z_des;
+
+        % Volterra Lotka
+%         u(1,:) = params.K1*(drive(1)-params.target(1));
+%         u(2,:) = params.K2*(drive(1)-params.target(1));
+%         % sat
+%         u(1,:) = min(max(-params.umax,u(1,:)),params.umax);
+%         u(2,:) = min(max(-params.umax,u(2,:)),params.umax);
     end
+
 end
\ No newline at end of file
diff --git a/Lib/control/control_TCV.m b/Lib/control/control_TCV.m
new file mode 100644
index 0000000..52baab2
--- /dev/null
+++ b/Lib/control/control_TCV.m
@@ -0,0 +1,28 @@
+%% CONTROL
+% file: control.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function implements the control law
+% INPUT:
+% t: time instant
+% drive: error variable
+% params: structure with all the necessary parameters 
+% OUTPUT:
+% u: control variable
+function u = control_TCV(t,drive,params,obs)
+
+    % check if the input is enabled
+    if params.input_enable
+        
+        % !REMARK!
+        % simply uncomment or add the law you need, according to params
+
+%         for i=1:params.m
+%             tmpstr =  ['u(', num2str(i) ',:) = ', num2str(i), '*sin(', num2str(i), '*t);'];
+%             eval(tmpstr); 
+%         end            
+        u(:,1) = ones(1,length(t));
+    else
+        u = zeros(params.dim_input,1);
+    end
+end
\ No newline at end of file
diff --git a/Lib/control/control_battery.m b/Lib/control/control_battery.m
new file mode 100644
index 0000000..4188ede
--- /dev/null
+++ b/Lib/control/control_battery.m
@@ -0,0 +1,33 @@
+%% CONTROL
+% file: control.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function implements the control law
+% INPUT:
+% t: time instant
+% drive: error variable
+% params: structure with all the necessary parameters 
+% OUTPUT:
+% u: control variable
+function u = control_battery(t,drive,params,obs)
+
+    % init input
+    u = zeros(1,length(t));
+    % params.dim_input
+
+    % compute the time index
+    for i=1:length(t)
+        tdiff = obs.setup.time-t(i);   
+        pos(i) = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+        pos(i) = max(1,pos(i));        
+    end 
+
+    % check if the input is enabled
+    if params.input_enable
+        
+        % from input data        
+        u = params.u_sim(:,pos);
+        
+           
+    end
+end
\ No newline at end of file
diff --git a/Lib/control/drive.m b/Lib/control/drive.m
new file mode 100644
index 0000000..3a78793
--- /dev/null
+++ b/Lib/control/drive.m
@@ -0,0 +1,78 @@
+%% control drive function (observer or control design)
+function drive_out = drive(varargin) 
+
+    obj = varargin{1};   
+
+    % init drive
+    drive_out = [];
+
+    % x - varargin
+    x_star = varargin{2}(1:obj.init.params.dim_state,:);            
+    x = varargin{3}(1:obj.init.params.dim_state,:);      
+
+
+    % just x
+%     drive_out = [drive_out; x(obj.setup.plot_vars)];
+    drive_out = [drive_out; x];
+
+    % get y    
+    if 1    
+    y_meas = varargin{4}(:,:,end);
+    pos = varargin{5};
+    
+    y = obj.setup.measure(x,obj.init.params,pos,obj.init.input_story(obj.init.traj).val(:,max(1,pos-1)));    
+    
+    %%% compute filters %%%
+    y_filt = [];
+    y_filt_meas = [];
+    if 1 && obj.setup.Nfilt > 0                     
+
+        % how long iterate        
+        tspan_pos = [max(1,pos-1), pos];                
+        
+        % update buffer - only first (RK4)
+        obj.init.Y_buffer_control(obj.init.traj).val(1,:,pos) = y;                        
+        
+        try
+            ok = 1;
+            [Y_filt, x_filter] = obj.measure_filter(obj.init.Y_buffer_control(obj.init.traj).val,tspan_pos,obj.init.X_filter_buffer_control(obj.init.traj));
+        catch            
+            ok=0;
+        end
+
+        if ok
+            for filt=1:obj.setup.Nfilt
+                for dim=1:obj.setup.dim_out
+                    % Lsim
+                    y_filt(filt,dim) = Y_filt{filt,dim}.val(end);
+                    obj.init.X_filter_buffer_control(obj.init.traj).val{filt,dim}(:,unique(tspan_pos)) = x_filter{filt,dim}.val;
+                                        
+                end
+                
+                % update buffer - only first (RK4)
+                obj.init.Y_buffer_control(obj.init.traj).val(1+filt,:,pos) = y_filt(filt,:);
+            end
+            
+            y_filt = reshape(y_filt,size(y));
+            y_pad = zeros(size(y).*[2,1]);
+            y_pad(1:2:end) = y;
+            y_pad(2:2:end) = y_filt;
+            y = y_pad;
+            
+        else
+%             y_meas = reshape(y_meas,size(y_meas,1)*size(y_meas,2));
+            y = y_meas;            
+        end
+    end
+          
+        y_meas = reshape(y_meas,size(y));
+        tmp = y_meas-y;        
+        tmp = reshape(tmp,length(y),1);
+
+        drive_out = [drive_out; tmp];
+    end
+    
+    % save drive story
+    obj.init.drive_out(obj.init.traj).val(:,pos) = drive_out;
+
+end
\ No newline at end of file
diff --git a/Lib/control/generate_HCCP.m b/Lib/control/generate_HCCP.m
new file mode 100644
index 0000000..8ff9cde
--- /dev/null
+++ b/Lib/control/generate_HCCP.m
@@ -0,0 +1,18 @@
+%% 
+function [HCCP, tspan, tspan_pos] = generate_HCCP(Ts,Cnh)
+
+    % time span
+    tspan = 0:Ts:60;
+    tspan_pos = 1:length(tspan);
+    
+    % intervals
+    tstops = [tspan_pos(1) find(tspan==20) find(tspan==50) tspan_pos(end)];
+    tamplitudes = [1*Cnh 0 -0.5*Cnh];
+    
+    % generate wave
+    HCCP = zeros(size(tspan));
+    for i=1:length(tamplitudes)
+       HCCP(tstops(i):tstops(i+1)) = tamplitudes(i); 
+    end
+
+end
\ No newline at end of file
diff --git a/Lib/integration/odeDD.m b/Lib/integration/odeDD.m
index 5dcd266..64917b4 100644
--- a/Lib/integration/odeDD.m
+++ b/Lib/integration/odeDD.m
@@ -11,6 +11,7 @@
 function out = odeDD(varargin)
 
 % init params
+nargin = length(varargin);
 
 % handle function to the dynamics
 ode = varargin{1};
@@ -21,17 +22,16 @@
 % y0: initial condition
 x0 = varargin{3};
 
-% U: input
-U = varargin{4};
-
-% TF: transfer function
-TF = varargin{5};
+% integrations options
+if nargin > 3
+    opts = varargin{4};
+end
 
 % number of state's components
 N = length(x0);           
   
 % numer of time steps
-M = length(tspan);   
+M = length(tspan);
 
 % initial time instant
 t0 = tspan(1);
@@ -39,17 +39,15 @@
 % Matrices allocation
 X = zeros(N,M);
 X(:,1) = x0;
-Y(:,1) = 0*U(:,1);
 
 % integration
 for i = 1:M-1
     
     % DD step
-    [Y(:,i+1),X(:,i+1)] = feval(ode,tspan(i),X(:,i),U,TF.A, TF.B, TF.C, TF.D);   
+    X(:,i+1) = feval(ode,tspan(i),X(:,i));   
     
 end
 
 % store
-out.x = X;
-out.y = Y(:,2:end);
+out.y = X;
 end
\ No newline at end of file
diff --git a/Lib/integration/odeEuler.m b/Lib/integration/odeEuler.m
new file mode 100644
index 0000000..7ad721f
--- /dev/null
+++ b/Lib/integration/odeEuler.m
@@ -0,0 +1,58 @@
+%% ODERK4_FAST
+% file: oderk4_fast.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function implements a 4th order Runge-Kutta integration
+% algorithm
+% INPUT:
+% ode: handle function to the dynamics
+% tspan: time interval for the integration
+% y0: initial condition
+% options: integration options (see ode45 help)
+% varargin: additional parameters (see options)
+% OUTPUT:
+% out: structure with integration results
+function out = odeEuler(ode,tspan,y0,options,varargin)
+
+%%%% init section %%%%
+
+% number of state's components
+N = length(y0);            
+
+% numer of time steps
+M = length(tspan);         
+
+% time step
+dt = tspan(2) - tspan(1);   
+
+% initial time instant
+t0 = tspan(1);
+
+% Matrices allocation
+X = zeros(N,M);
+X(:,1) = y0;
+K1 = zeros(N,M);
+
+% integration
+for i = 1:M-1
+    
+    % State and time at ti
+    x = X(:,i);
+    
+    % Runge Kutta 4
+    [K1(:,i), Xjump] = feval(ode,t0,x);  
+
+    %%% just to update the drive correctly %%%
+    %tmp = feval(ode,t0,x);
+    
+    % Solution at ti+1
+    X(:,i+1) = Xjump + dt*K1(:,i);
+    
+    % shift time instant
+    t0 = tspan(i+1);
+    
+end
+
+% store
+out.y = X;
+end
\ No newline at end of file
diff --git a/Lib/integration/odeLsim.m b/Lib/integration/odeLsim.m
new file mode 100644
index 0000000..f900278
--- /dev/null
+++ b/Lib/integration/odeLsim.m
@@ -0,0 +1,23 @@
+%% ODERK4_FAST
+% file: oderk4_fast.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function implements a 4th order Runge-Kutta integration
+% algorithm
+% INPUT:
+% ode: handle function to the dynamics
+% tspan: time interval for the integration
+% y0: initial condition
+% options: integration options (see ode45 help)
+% varargin: additional parameters (see options)
+% OUTPUT:
+% out: structure with integration results
+function out = odeLsim(ode,tspan,y0,options,varargin)
+
+%%%% init section %%%%
+out.y(:,1) = y0;
+Nstep = length(tspan);
+
+% store
+out.y(:,2:Nstep) = feval(ode,tspan,y0);
+end
\ No newline at end of file
diff --git a/Lib/integration/odeTF.m b/Lib/integration/odeTF.m
new file mode 100644
index 0000000..dd62d57
--- /dev/null
+++ b/Lib/integration/odeTF.m
@@ -0,0 +1,56 @@
+%% ODEDD
+% file: odeDD.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function implements a discrete time system integration
+% algorithm
+% INPUT:
+% varargin: general parameters
+% OUTPUT:
+% out: structure with integration results
+function out = odeTF(varargin)
+
+% init params
+nargin = length(varargin);
+
+% handle function to the dynamics
+ode = varargin{1};
+
+% tspan: time interval for the integration
+tspan = varargin{2};
+
+% y0: initial condition
+x0 = varargin{3};
+
+% U: input
+U = varargin{4};
+
+% TF: transfer function
+TF = varargin{5};
+
+% number of state's components
+N = length(x0);           
+  
+% numer of time steps
+M = length(tspan);   
+
+% initial time instant
+t0 = tspan(1);
+
+% Matrices allocation
+X = zeros(N,M);
+X(:,1) = x0;
+Y(:,1) = 0*U(:,1);
+
+% integration
+for i = 1:M-1
+    
+    % DD step
+    [Y(:,i+1),X(:,i+1)] = feval(ode,tspan(i),X(:,i),U,TF.A, TF.B, TF.C, TF.D);   
+    
+end
+
+% store
+out.x = X;
+out.y = Y(:,2:end);
+end
\ No newline at end of file
diff --git a/Lib/integration/oderk4_fast.m b/Lib/integration/oderk4_fast.m
index 74c39b4..9d71ac7 100644
--- a/Lib/integration/oderk4_fast.m
+++ b/Lib/integration/oderk4_fast.m
@@ -41,9 +41,12 @@
     
     % Runge Kutta 4
     K1(:,i) = feval(ode,t0,x);
-    K2 = feval(ode,t0,x + K1(:,i)*dt/2);
-    K3 = feval(ode,t0,x + K2*dt/2);
-    K4 = feval(ode,t0,x + K3*dt);
+    K2 = feval(ode,t0+dt/2,x + K1(:,i)*dt/2);
+    K3 = feval(ode,t0+dt/2,x + K2*dt/2);
+    K4 = feval(ode,t0+dt,x + K3*dt);
+    
+    %%% just to update the drive correctly %%%
+    tmp = feval(ode,t0,x);
     
     % Solution at ti+1
     X(:,i+1) = x + (dt/6)*(K1(:,i) + 2*K2 + 2*K3 + K4);
diff --git a/Lib/measure/.DS_Store b/Lib/measure/.DS_Store
new file mode 100644
index 0000000..a9737e0
Binary files /dev/null and b/Lib/measure/.DS_Store differ
diff --git a/Lib/measure/Simulink/measure_battery_tushar_SIM.m b/Lib/measure/Simulink/measure_battery_tushar_SIM.m
new file mode 100644
index 0000000..1ea58e3
--- /dev/null
+++ b/Lib/measure/Simulink/measure_battery_tushar_SIM.m
@@ -0,0 +1,18 @@
+%% MEASURE_BATTERY
+% file: measure_battery.m
+% author: Federico Oliva
+% date: 27/05/2022
+% description: this function implements the output mapping of a battery
+% model
+% INPUT:
+% x: state vector
+% params: structure with all the necessary parameters 
+% t: time instant (may be not used)
+% OUTPUT:
+% y: output measurement
+function y = measure_battery_tushar_SIM(x,u,t)    
+    
+    % get the observed components of the state vector
+    y(1,:) = x(3,:) - x(2,:) -(x(4,:).*u);
+
+end
\ No newline at end of file
diff --git a/Lib/measure/filter_define.m b/Lib/measure/filter_define.m
index 1635d68..5350400 100644
--- a/Lib/measure/filter_define.m
+++ b/Lib/measure/filter_define.m
@@ -4,7 +4,7 @@
 % date: 10/01/2022
 % description: this function defines the filters applied to the output 
 % measurements. Their usage is strictly related to filtered_MHE (see 
-% https://doi.org/10.48550/arXiv.2204.09359). Simply use the if 1
+% https://doi.org/10.48550/arXiv.2204.09359). Simply use the if 0;
 % enable or disable the filtering actions.
 % INPUT: 
 % Ts: sampling time
@@ -20,12 +20,13 @@
     filter = [];
     
     %%% derivative filter %%%
-    if 0
-    i = 1;   
-    eps1 = 1e-1;
-    G = tf([1 0],[eps1 1]);
+    fil1 = 0;
+    if fil1
+    i = i+1;   
+    eps1 = 1e-0;    
+    G = tf([1 0],[eps1 1]);    
     SS = ss(G);
-    D = c2d(SS,Ts);
+    D = c2d(SS,Nts*Ts);
     filter(i).TF = D;
     filter(i).A = D.A;
     filter(i).B = D.B;
@@ -37,9 +38,10 @@
     end
 
     %%%% integral filter %%%%
-    if 0
-    i = 2;    
-    eps2 = 1e2;
+    fil2 = 0;
+    if fil2
+    i = i+1;    
+    eps2 = 1e5;
     G = tf(1,[eps2 1]);
     SS = ss(G);
     D = c2d(SS,Ts);
@@ -53,10 +55,33 @@
     filterScale(i+1)= 1;
     end
     
+    %%%% 2nd order filter %%%%
+    fil3 = 0;
+    if fil3
+    i = i+1; 
+    eps0 = 0*1e-4;
+    eps1 = 1e2;
+    K = 1;
+    eps2 = 1e4;
+    G1 = tf(K*[eps0 1],[eps1 1]);
+    G2 = tf([1],[eps2 1]);
+    G = G1*G2;
+    SS = ss(G1);
+    D = c2d(SS,Ts);
+    filter(i).TF = D;
+    filter(i).A = D.A;
+    filter(i).B = D.B;
+    filter(i).C = D.C;
+    filter(i).D = D.D;
+    filter(i).G = G;
+    filter(i).dim = size(D.B,1);
+    filterScale(i+1)= 1;
+    end
+    
     %%%% reference filter %%%%
     % (under development)
-    eps = 2e0;
-    G = tf(eps,[1 eps]);
+    eps = 2;
+    G = tf(1,[eps 1]);
     SS = ss(G);
     D = c2d(SS,Ts);
     reference.TF = D;
@@ -66,5 +91,9 @@
     reference.D = D.D;
     reference.G = G;
     reference.dim = size(D.B,1);
+    reference.x0 = 4*ones(reference.dim,1);
+    
+    %%% butterworth %%%
+    [reference.butter.b,reference.butter.a] = butter(1,0.8);
     
 end
\ No newline at end of file
diff --git a/Lib/measure/measure_TCV_reference.m b/Lib/measure/measure_TCV_reference.m
new file mode 100644
index 0000000..17af778
--- /dev/null
+++ b/Lib/measure/measure_TCV_reference.m
@@ -0,0 +1,20 @@
+%% MODEL_REFERENCE
+% file: measure_control_test.m
+% author: Federico Oliva
+% date: 22/06/2022
+% description: this function implements the output mapping of a general
+% state space system
+% INPUT:
+% x: state vector
+% params: structure with all the necessary parameters 
+% t: time instant (may be not used)
+% OUTPUT:
+% y: output measurement
+function y = measure_TCV_reference(x,params,t,u)
+    
+    
+    % get the observed components of the state vector 
+    range = params.dim_state_r+params.dim_state_c+params.dim_state_op+params.dim_state_an+1:params.dim_state_r+params.dim_state_c+params.dim_state_op+params.dim_state_an+params.n;
+    y = params.C*x(range,:) + params.D*u;
+    
+end
\ No newline at end of file
diff --git a/Lib/measure/measure_armesto.m b/Lib/measure/measure_armesto.m
new file mode 100644
index 0000000..f01cb61
--- /dev/null
+++ b/Lib/measure/measure_armesto.m
@@ -0,0 +1,29 @@
+%% MODEL_REFERENCE
+% file: measure_general.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function implements the output mapping of a general
+% state space system
+% INPUT:
+% x: state vector
+% params: structure with all the necessary parameters 
+% t: time instant (may be not used)
+% OUTPUT:
+% y: output measurement
+function [y, obs] = measure_armesto(x,params,t,u,obs)
+
+    % compute the time index
+    pos = zeros(1,length(t));
+    for i=1:length(t)
+        tdiff = obs.setup.time-t(i);   
+        pos(i) = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+        pos(i) = max(1,pos(i));        
+    end
+
+    % get the observed components of the state vector
+    y = x(params.observed_state,:);    
+
+    % add bias
+    y(params.pos_biased_out) = y(params.pos_biased_out) + x(params.pos_bias);
+    
+end
\ No newline at end of file
diff --git a/Lib/measure/measure_armesto_reference.m b/Lib/measure/measure_armesto_reference.m
new file mode 100644
index 0000000..eb20233
--- /dev/null
+++ b/Lib/measure/measure_armesto_reference.m
@@ -0,0 +1,60 @@
+%% MODEL_REFERENCE
+% file: measure_general.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function implements the output mapping of a general
+% state space system
+% INPUT:
+% x: state vector
+% params: structure with all the necessary parameters 
+% t: time instant (may be not used)
+% OUTPUT:
+% y: output measurement
+function [y, obs] = measure_armesto_reference(x,params,t,u,obs)
+
+    % compute the time index
+    pos = zeros(1,length(t));
+    for i=1:length(t)
+        tdiff = obs.setup.time-t(i);   
+        pos(i) = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+        pos(i) = max(1,pos(i));        
+    end
+
+    % get traj
+    traj = obs.init.traj;
+
+    % get the observed components of the state vector
+    y_true = x(params.observed_state,:);    
+
+    % noise
+    noise = obs.setup.noise*(params.noise_mat(:,2).*randn(obs.setup.dim_out,1) + params.noise_mat(:,1));
+    noise(params.pos_biased_out) = noise(params.pos_biased_out) + x(params.pos_bias);
+    y = y_true + noise;
+    y(params.pos_quat_out) = quatnormalize(y(params.pos_quat_out)');
+
+    % down sampling - CAM
+    if mod(pos,params.CAM_samp) ~= 0
+        y(params.pos_p_out) = obs.init.params.last_CAM_pos(traj,:);
+        y(params.pos_quat_out) = obs.init.params.last_CAM_quat(traj,:);
+        noise([params.pos_p_out params.pos_quat_out]) = obs.init.params.last_noise(traj,[params.pos_p_out params.pos_quat_out]);
+    end
+
+    % down sampling - IMU
+    if mod(pos,params.IMU_samp) ~= 0
+        y(params.pos_acc_out) = obs.init.params.last_IMU_acc(traj,:);
+        y(params.pos_omega_out) = obs.init.params.last_IMU_omega(traj,:);
+        noise([params.pos_acc_out params.pos_omega_out]) = obs.init.params.last_noise(traj,[params.pos_acc_out params.pos_omega_out]);
+    end
+
+    % store
+    obs.init.params.last_noise(traj,:) = noise;
+    obs.init.params.last_CAM_pos(traj,:) = y(params.pos_p_out);
+    obs.init.params.last_CAM_quat(traj,:) = y(params.pos_quat_out);
+    obs.init.params.last_IMU_acc(traj,:) = y(params.pos_acc_out);
+    obs.init.params.last_IMU_omega(traj,:) = y(params.pos_omega_out);
+
+    % store
+    obs.init.Ytrue_full_story(traj).val(1,:,pos) = y_true;    
+    obs.init.noise_story(traj).val(:,pos) = noise;
+    obs.init.Y_full_story(traj).val(1,:,pos) = y;
+end
\ No newline at end of file
diff --git a/Lib/measure/measure_battery_tushar.m b/Lib/measure/measure_battery_tushar.m
new file mode 100644
index 0000000..0fe2c7b
--- /dev/null
+++ b/Lib/measure/measure_battery_tushar.m
@@ -0,0 +1,21 @@
+%% MEASURE_BATTERY
+% file: measure_battery.m
+% author: Federico Oliva
+% date: 27/05/2022
+% description: this function implements the output mapping of a battery
+% model
+% INPUT:
+% x: state vector
+% params: structure with all the necessary parameters 
+% t: time instant (may be not used)
+% OUTPUT:
+% y: output measurement
+function y = measure_battery_tushar(x,params,t,u,obs)
+
+    % compute the control - I
+    params.u = params.input(t,x,params,obs);    
+    
+    % get the observed components of the state vector
+    y(1,:) = x(3,:) - x(2,:) -(x(4,:).*params.u(1,:));
+%     y(2,:) = x(10,:);
+end
\ No newline at end of file
diff --git a/Lib/measure/measure_general.m b/Lib/measure/measure_general.m
index 5c78c0e..64027e6 100644
--- a/Lib/measure/measure_general.m
+++ b/Lib/measure/measure_general.m
@@ -10,7 +10,7 @@
 % t: time instant (may be not used)
 % OUTPUT:
 % y: output measurement
-function y = measure_general(x,params,t)
+function [y, obs] = measure_general(x,params,t,u,obs)
 
     % get the observed components of the state vector
     y = x(params.observed_state,:);
diff --git a/Lib/measure/measure_rover.m b/Lib/measure/measure_rover.m
new file mode 100644
index 0000000..55eb408
--- /dev/null
+++ b/Lib/measure/measure_rover.m
@@ -0,0 +1,74 @@
+%% MODEL_REFERENCE
+% file: measure_general.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function implements the output mapping of a general
+% state space system
+% INPUT:
+% x: state vector
+% params: structure with all the necessary parameters 
+% t: time instant (may be not used)
+% OUTPUT:
+% y: output measurement
+function y = measure_rover(x,params,tspan,u,obs) 
+
+    %%% when do I stop? %%%
+    t = tspan;
+    if numel(t)>2
+        stopK = numel(t);
+        offset_UWBsamp = 0;
+    else
+        stopK = max(1,numel(t)-1);
+        offset_UWBsamp = 1;
+    end
+
+    % define y
+    y = zeros(params.OutDim,stopK);
+
+    % get traj
+    traj = obs.init.traj;       
+
+    % compute the time index
+    pos = zeros(1,length(t));
+    for i=1:length(t)
+        tdiff = obs.setup.time-t(i);   
+        pos(i) = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+        pos(i) = max(1,pos(i));        
+    end
+    
+    for k=1:stopK
+        
+    
+        %%% get the output mismatch terms        
+        V_true = reshape(x(params.pos_v,k),numel(params.pos_v),1);
+        P_true = reshape(x(params.pos_p,k),numel(params.pos_p),1);
+    
+        %%% get distances        
+        if mod(pos(k)+offset_UWBsamp,params.UWB_samp) == 0                                    
+            % adjacency matrix
+            for dim=1:params.space_dim
+                Pa(dim,:) = x(params.pos_anchor(dim):params.space_dim:params.pos_anchor(end));            
+            end
+            % true distances
+            D = get_dist(P_true,Pa);   
+            obs.init.params.last_D(traj,:) = D;
+        else   
+            D = reshape(obs.init.params.last_D(traj,:),params.Nanchor,1);
+        end
+    
+        %%% get the IMU accelerations
+        if mod(pos(k)+offset_UWBsamp,params.IMU_samp) == 0                 
+            xd = obs.setup.model([t(k) t(k)+params.Ts],x,params,obs);        
+            IMU_true = reshape(xd(params.pos_v,:),numel(params.pos_v),1);          
+            obs.init.params.last_IMU_acc(traj,:) = IMU_true;
+        else       
+            IMU_true = reshape(obs.init.params.last_IMU_acc(traj,:),params.space_dim,1);
+        end
+    
+        % add bias on IMU
+        IMU_true = IMU_true;
+
+        % final measure
+        y(:,k) = [D; P_true; V_true; IMU_true];                     
+    end
+end
\ No newline at end of file
diff --git a/Lib/measure/measure_rover_reference.m b/Lib/measure/measure_rover_reference.m
new file mode 100644
index 0000000..7e85a2c
--- /dev/null
+++ b/Lib/measure/measure_rover_reference.m
@@ -0,0 +1,114 @@
+%% MODEL_REFERENCE
+% file: measure_general.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function implements the output mapping of a general
+% state space system
+% INPUT:
+% x: state vector
+% params: structure with all the necessary parameters 
+% t: time instant (may be not used)
+% OUTPUT:
+% y: output measurement
+function [y, obs] = measure_rover_reference(x,params,t,u,obs)        
+
+    % compute the time index
+    pos = zeros(1,length(t));
+    for i=1:length(t)
+        tdiff = obs.setup.time-t(i);   
+        pos(i) = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+        pos(i) = max(1,pos(i));        
+    end
+
+    % get traj
+    traj = obs.init.traj;
+
+    %%% get the output mismatch terms    
+    V_true = reshape(x(params.pos_v,:),numel(params.pos_v),size(x,2));
+    P_true = reshape(x(params.pos_p,:),numel(params.pos_p),size(x,2));
+
+    % different sampling times
+    if mod(pos(end),params.UWB_samp) == 0
+        %%% get distances        
+        % adjacency matrix
+        for dim=1:params.space_dim
+            Pa(dim,:) = x(params.pos_anchor(dim):params.space_dim:params.pos_anchor(end));            
+        end
+        
+        % true distances
+        D = get_dist(P_true,Pa);
+        % save position buffer
+        obs.init.params.UWB_pos(end+1) = pos(end);
+        obs.init.params.last_D_ref(traj,:) = D;
+    else
+        D = reshape(obs.init.params.last_D_ref(traj,:),params.Nanchor,1);
+    end    
+
+    %%% get the IMU accelerations
+    if mod(pos(end),params.IMU_samp) == 0 
+        % save story
+        old_u = obs.init.input_story_ref(traj).val(:,pos(1));
+        old_buffer = obs.init.params.err_der_buffer;
+        old_counter = obs.init.params.err_der_counter;
+
+        % compute
+        xd = obs.setup.model_reference(t,x,params,obs);
+
+        % restore
+        obs.init.input_story_ref(traj).val(:,pos(1)) = old_u;
+        obs.init.params.err_der_buffer = old_buffer;
+        obs.init.params.err_der_counter = old_counter;
+
+        % meas
+        IMU_true = reshape(xd(params.pos_v,:),numel(params.pos_v),size(xd,2));
+        obs.init.params.last_IMU_acc_ref(traj,:) = IMU_true;
+    else       
+        IMU_true = reshape(obs.init.params.last_IMU_acc_ref(traj,:),params.space_dim,1);
+    end
+
+    %%% add noise
+    % noise on UWB + IMU
+    y_true = [D; P_true; V_true; IMU_true];
+    noise = obs.setup.noise*(params.noise_mat(:,1).*randn(obs.setup.dim_out,1));
+
+    % bias IMU
+    noise(params.pos_acc_out) = noise(params.pos_acc_out) + 1*x(params.pos_bias);
+
+    %%% multi rate - UWB
+    if mod(pos(end),params.UWB_samp) ~= 0
+        try
+            noise(params.pos_dist_out) = obs.init.noise_story(traj).val(params.pos_dist_out,pos(1)-1);
+        catch
+            noise(params.pos_dist_out) = 0;
+        end
+    end
+
+    %%% multi rate - IMU
+    if mod(pos(end),params.IMU_samp) ~= 0
+        try
+            noise(params.pos_acc_out) = obs.init.noise_story(traj).val(params.pos_acc_out,pos(1)-1);
+        catch
+            noise(params.pos_acc_out) = 0;
+        end
+    end
+
+    %%% add noise
+    y = y_true + noise;    
+
+    %%% OPT - pjump %%%
+    if mod(pos(end),params.UWB_samp) == 0
+        D_meas = y(params.pos_dist_out);
+        obs.init.params.p_jump(traj).val(:,end+1) = fminunc(@(x)J_dist(x,Pa,D_meas),x(params.pos_p),obs.setup.params.dist_optoptions);
+        [obs.init.params.p_jump_der(traj).val(:,end+1), obs.init.params.p_jump_der_buffer, obs.init.params.p_jump_der_counter(traj).val] = PseudoDer(params.Ts*params.UWB_samp,...
+            obs.init.params.p_jump(traj).val(:,end),params.wlen,...
+            params.buflen,params.space_dim,0,0,obs,obs.init.params.p_jump_der_buffer,obs.init.params.p_jump_der_counter(traj).val);
+        if traj == 1
+            obs.init.params.p_jump_time(end+1) = pos(end);
+        end
+    end
+    
+    % store
+    obs.init.Ytrue_full_story(traj).val(1,:,pos(end)) = y_true;    
+    obs.init.noise_story(traj).val(:,pos(end)) = noise;
+    obs.init.Y_full_story(traj).val(1,:,pos(end)) = y;
+end
\ No newline at end of file
diff --git a/Lib/models/.DS_Store b/Lib/models/.DS_Store
new file mode 100644
index 0000000..4c96833
Binary files /dev/null and b/Lib/models/.DS_Store differ
diff --git a/Lib/models/TCV/DotXopt.m b/Lib/models/TCV/DotXopt.m
new file mode 100644
index 0000000..c1a1fed
--- /dev/null
+++ b/Lib/models/TCV/DotXopt.m
@@ -0,0 +1,19 @@
+%%
+function xdot = DotXopt(x,yc,params)
+
+    % init
+    xdot = zeros(params.dim_state_op,1);
+    Gamma = params.Gamma;
+    Anstar = params.Anstar;  
+    range = params.dim_state_r+params.dim_state_c+1:params.dim_state_r+params.dim_state_c+params.dim_state_op;
+    xop = x(range);
+    u = yc + Anstar*xop;
+
+    for i=1:params.m
+        if abs(u(i))>params.InputBound
+            Grad = params.R(i)*(u(i) - params.InputBound)*Anstar(i,:);
+            xdot = xdot-Gamma*Grad';
+        end
+    end
+
+end
\ No newline at end of file
diff --git a/Lib/models/TCV/General/model_TCV.m b/Lib/models/TCV/General/model_TCV.m
new file mode 100644
index 0000000..16be47e
--- /dev/null
+++ b/Lib/models/TCV/General/model_TCV.m
@@ -0,0 +1,91 @@
+%% MODEL_OSCILLATOR_VDP
+% file: model_control_test.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function describes the dynamics equation of an unstable
+% LTI model
+% INPUT:
+% t: time instant
+% x: state vector
+% params: structure with all the necessary parameters 
+% obs: observer class instance (may be not used)
+% OUTPUT:
+% x_dot: dynamics equations
+function x_dot = model_TCV(t,x,params,obs)
+
+    % init the dynamics 
+    x_dot = zeros(length(x),1);
+    
+    % compute the time index
+    tdiff = obs.setup.time-t;   
+    pos = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+    pos = max(1,min(pos,size(obs.init.Y_full_story(obs.init.traj).val,3)));
+    
+    % compute y out of p
+    y = obs.setup.measure(x,params,t,obs.init.input_story(obs.init.traj).val(:,max(1,pos-1)));
+    
+    % compute the reference (Sigma_r)
+    range = 1:params.dim_state_r;
+    x_dot(range,:) = params.Ar*x(range,:) + params.Br*0; 
+    r = params.Cr*x(range,:) + params.Dr*0;    
+    
+    % compute the control (Sigma_c)
+    range = params.dim_state_r+1:params.dim_state_r+params.dim_state_c;
+    rbar = r(params.q_pos);
+    ybar = y(params.q_pos);
+    if params.Zaccarian
+        e = ybar;
+    else
+        e = rbar-ybar;    
+    end
+    obs.init.error_story_ref(obs.init.traj).val(:,pos) = e;
+    x_dot(range,:) = params.Ac*x(range,:) + params.Bc*e + params.Bcr*rbar; 
+    u = params.Cc*x(range,:) + params.Dc*e + params.Dcr*rbar;           
+    obs.init.input_default_story(obs.init.traj).val(:,pos) = u;  
+    
+    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+    %%%%%%%%%%%% ALLOCATION %%%%%%%%%%%
+    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+    
+    %%%%%% compute the optimizer %%%%%%
+    range = params.dim_state_r+params.dim_state_c+1:params.dim_state_r+params.dim_state_c+params.dim_state_op;
+    ytilde = y(params.eta_pos);        
+    % gradient
+    if params.staticAll
+        % static grad
+        grad = params.R*params.Pstar_tilde*params.Pperp_bar;
+    else
+        % dynamic grad
+        grad = params.R*params.Pstar_tilde*params.Anstar;
+    end        
+    if params.Zaccarian
+        % static grad
+        grad = params.Rzac*transpose(params.Bperp)*params.W;
+        x_dot(range,:) = params.gamma*grad*u;
+    else
+        x_dot(range,:) = params.gamma*transpose(ytilde)*grad;
+    end        
+    v = x(range);
+    obs.init.optimizer_story(obs.init.traj).val(:,pos) = v;
+    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+    
+    %%%%%% compute the annihilator %%%%%%
+    range = params.dim_state_r+params.dim_state_c+params.dim_state_op+1:params.dim_state_r+params.dim_state_c+params.dim_state_op+params.dim_state_an;
+    x_dot(range,:) = params.A_an*x(range,:) + params.B_an*v;
+    if params.Zaccarian
+        ya = params.D_an_zac*v;
+    else
+        ya = params.C_an*x(range,:) + params.D_an*v;
+    end
+    obs.init.allocator_story(obs.init.traj).val(:,pos) = ya;
+        
+    % sum input
+%     u_new = u;
+    u_new = u+ya;
+    obs.init.input_story(obs.init.traj).val(:,pos) = u_new;  
+    
+    % model dynamics  (Sigma p)
+    range = params.dim_state_r+params.dim_state_c+params.dim_state_op+params.dim_state_an+1:params.dim_state_r+params.dim_state_c+params.dim_state_op+params.dim_state_an+params.n;
+    x_dot(range,:) = params.sys_pert(obs.init.traj).A*x(range,:) + params.sys_pert(obs.init.traj).B*(u_new);    
+       
+end
\ No newline at end of file
diff --git a/Lib/models/TCV/General/model_TCV_noAll.m b/Lib/models/TCV/General/model_TCV_noAll.m
new file mode 100644
index 0000000..9a25bef
--- /dev/null
+++ b/Lib/models/TCV/General/model_TCV_noAll.m
@@ -0,0 +1,50 @@
+%% MODEL_OSCILLATOR_VDP
+% file: model_control_test.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function describes the dynamics equation of an unstable
+% LTI model
+% INPUT:
+% t: time instant
+% x: state vector
+% params: structure with all the necessary parameters 
+% obs: observer class instance (may be not used)
+% OUTPUT:
+% x_dot: dynamics equations
+function x_dot = model_TCV_noAll(t,x,params,obs)
+
+    % init the dynamics 
+    x_dot = zeros(length(x),1);
+    
+    % compute the time index
+    tdiff = obs.setup.time-t;   
+    pos = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+    pos = max(1,min(pos,size(obs.init.Y_full_story(obs.init.traj).val,3)));
+    
+    % compute y out of p
+    y = obs.setup.measure(x,params,t,obs.init.input_story(obs.init.traj).val(:,max(1,pos-1)));
+    
+    % compute the reference (Sigma_r)
+    range = 1:params.dim_state_r;
+    x_dot(range,:) = params.Ar*x(range,:) + params.Br*0; 
+    r = params.Cr*x(range,:) + params.Dr*0;    
+    
+    % compute the control (Sigma_c)
+    range = params.dim_state_r+1:params.dim_state_r+params.dim_state_c;
+    rbar = r(params.q_pos);
+    ybar = y(params.q_pos);
+    if params.Zaccarian
+        e = ybar;
+    else
+        e = rbar-ybar;    
+    end   
+    x_dot(range,:) = params.Ac*x(range,:) + params.Bc*e + params.Bcr*rbar; 
+    u = params.Cc*x(range,:) + params.Dc*e + params.Dcr*rbar;            
+    obs.init.input_default_story(obs.init.traj).val(:,pos) = u;          
+    obs.init.input_story(obs.init.traj).val(:,pos) = u;  
+    
+    % model dynamics  (Sigma p)
+    range = params.dim_state_r+params.dim_state_c+params.dim_state_op+params.dim_state_an+1:params.dim_state_r+params.dim_state_c+params.dim_state_op+params.dim_state_an+params.n;
+    x_dot(range,:) = params.sys_pert(obs.init.traj).A*x(range,:) + params.sys_pert(obs.init.traj).B*(u);    
+       
+end
\ No newline at end of file
diff --git a/Lib/models/TCV/General/model_TCV_reference.m b/Lib/models/TCV/General/model_TCV_reference.m
new file mode 100644
index 0000000..981c20b
--- /dev/null
+++ b/Lib/models/TCV/General/model_TCV_reference.m
@@ -0,0 +1,57 @@
+%% MODEL_OSCILLATOR_VDP
+% file: model_control_test.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function describes the dynamics equation of an unstable
+% LTI model
+% INPUT:
+% t: time instant
+% x: state vector
+% params: structure with all the necessary parameters 
+% obs: observer class instance (may be not used)
+% OUTPUT:
+% x_dot: dynamics equations
+function x_dot = model_TCV_reference(t,x,params,obs)
+
+    % init the dynamics 
+    x_dot = zeros(length(x),1);
+    
+    % compute the time index
+    tdiff = obs.setup.time-t;   
+    pos = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+    pos = max(1,min(pos,size(obs.init.Y_full_story(obs.init.traj).val,3)));
+    
+    % compute y out of p
+    y = obs.setup.measure_reference(x,params,t,obs.init.input_story_ref(obs.init.traj).val(:,max(1,pos-1)));
+    
+    % compute the reference (Sigma_r)
+    range = 1:params.dim_state_r;
+    x_dot(range,:) = params.Ar*x(range,:) + params.Br*0; 
+    r = params.Cr*x(range,:) + params.Dr*0;
+    obs.init.reference_story(obs.init.traj).val(:,pos) = r;
+    
+    % compute the control (Sigma_c)
+    range = params.dim_state_r+1:params.dim_state_r+params.dim_state_c;
+    rbar = r(params.q_pos);
+    ybar = y(params.q_pos);
+    if params.Zaccarian
+        e = ybar;
+    else
+        e = rbar-ybar;    
+    end
+    obs.init.error_story_ref(obs.init.traj).val(:,pos) = e;
+    x_dot(range,:) = params.Ac*x(range,:) + params.Bc*e + params.Bcr*rbar; 
+    u = params.Cc*x(range,:) + params.Dc*e + params.Dcr*rbar;            
+    obs.init.input_story_ref(obs.init.traj).val(:,pos) = u;        
+    
+    % model dynamics  (Sigma p)
+    range = params.dim_state_r+params.dim_state_c+params.dim_state_op+params.dim_state_an+1:params.dim_state_r+params.dim_state_c+params.dim_state_op+params.dim_state_an+params.n;
+    x_dot(range,:) = params.A*x(range,:) + params.B*(u);   
+    
+    %%% only for safety %%%
+    if strcmp(func2str(obs.setup.model),func2str(obs.setup.model_reference))
+        obs.init.input_default_story(obs.init.traj).val(:,pos) = u;
+        obs.init.input_story(obs.init.traj).val(:,pos) = u;
+    end
+ 
+end
\ No newline at end of file
diff --git a/Lib/models/TCV/General/params_TCV.m b/Lib/models/TCV/General/params_TCV.m
new file mode 100644
index 0000000..a80e50f
--- /dev/null
+++ b/Lib/models/TCV/General/params_TCV.m
@@ -0,0 +1,227 @@
+    %% PARAMS_OSCILLATOR_VDP
+% file: params_control_test.m
+% author: Federico Oliva
+% date: 22/06/2022
+% description: this function initialises the parameters for an unstable LTI
+% model
+% INPUT: none
+% OUTPUT:
+% params: structure with all the necessary parameters
+function params = params_TCV(varargin)
+
+        
+    
+    % Zaccarian
+    params.Zaccarian = 1;
+    if params.Zaccarian
+        % generate system
+        params.A = [-0.157 -0.094; ...
+                    -0.416 -0.45];
+        params.B = [0.87 0.253 0.743; ...
+                    0.39 0.354 0.65];
+        params.C = [0 1];
+        params.D = [0 0 0];
+        params.sys_P = ss(params.A,params.B,params.C,params.D);
+        
+        % generate dimensions
+        params.n = size(params.A,1);
+        params.m = size(params.B,2);
+        params.p = size(params.C,1);
+        
+        % generate subsystem bar
+        params.q_pos = 1;    
+        params.q = length(params.q_pos);
+        params.Cbar = params.C(params.q_pos,:);
+        params.Dbar = params.D(params.q_pos,:);
+        params.sys_Pbar = ss(params.A,params.B,params.Cbar,params.Dbar);
+        
+        % generate subsystem tilde        
+        params.eta_pos = 1:params.p;
+        params.eta_pos(params.q_pos) = [];
+        params.eta = params.p-params.q;
+        params.Ctilde = params.C(params.eta_pos,:);        
+        params.Dtilde = params.D(params.eta_pos,:);        
+        params.sys_Ptilde = ss(params.A,params.B,params.Ctilde,params.Dtilde);
+                          
+    else
+    
+        %%% random sys %%
+        % generate system
+        params.sys_P = varargin{1}{2};
+        params.A = params.sys_P.A;
+        params.B = params.sys_P.B;    
+        params.C = params.sys_P.C;
+        params.D = params.sys_P.D;
+        
+        % generate dimensions
+        params.n = size(params.A,1);
+        params.m = size(params.B,2);
+        params.p = size(params.C,1);
+        
+        % generate subsystem bar
+        params.q_pos = varargin{1}{1};    
+        params.q = length(params.q_pos);
+        params.Cbar = params.C(params.q_pos,:);
+        params.Dbar = params.D(params.q_pos,:);
+        params.sys_Pbar = ss(params.A,params.B,params.Cbar,params.Dbar);
+        
+        % generate subsystem tilde        
+        params.eta_pos = 1:params.p;
+        params.eta_pos(params.q_pos) = [];
+        params.eta = params.p-params.q;
+        params.Ctilde = params.C(params.eta_pos,:);        
+        params.Dtilde = params.D(params.eta_pos,:);        
+        params.sys_Ptilde = ss(params.A,params.B,params.Ctilde,params.Dtilde);               
+        
+    end           
+    
+    % reference signal system (Sigma_r)
+    params.dim_state_r = size(params.C,1);
+    params.Ar = zeros(params.dim_state_r);
+    params.Br = 0;
+    params.Cr = eye(params.dim_state_r);
+    params.Dr = 0;    
+    
+                
+    % input stuff
+    params.dim_input = params.m;    
+    
+    % controller system (Sigma_c)
+    if params.Zaccarian
+        params.dim_state_c = 4;
+        params.Ac = [-1.57 0.5767 0.822 -0.65;...
+                     -0.9 -0.501 -0.94 0.802; ...
+                     0 1 -1.61 1.614; ...
+                     0 0 0 0];
+        params.Bcr = [0; 0; 0; 1];
+        params.Bc = -params.Bcr;
+        params.Cc = [1.81 -1.2 -0.46 0; ...
+                     -0.62 1.47 0.89 0; ...
+                     0 0 0 0];
+        params.Dcr = [0; 0; 0];
+        params.Dc = -params.Dcr;
+        params.sys_C = ss(params.Ac,[params.Bc params.Bcr],params.Cc,[params.Dc params.Dcr]);
+    else
+        params.dim_state_c = 1;    
+        params.Ac = zeros(params.dim_state_c);
+        params.Bc = zeros(params.dim_state_c,params.q);
+        params.Bcr = zeros(params.dim_state_c,params.q);
+        params.Cc = zeros(params.dim_input,params.dim_state_c);
+        params.Dc = eye(params.dim_input,params.q);
+        params.Dcr = zeros(params.dim_input,params.q);
+        params.sys_C = ss(params.Ac,[params.Bc params.Bcr],params.Cc,[params.Dc params.Dcr]);
+    end
+           
+    %compute annihilator        
+    params.sys_Pbar = ss(params.A,params.B,params.Cbar,params.Dbar);
+    params.sys_Ptilde = ss(params.A,params.B,params.Ctilde,params.Dtilde);
+        
+    % choose allocator
+    params.staticAll = varargin{1}{3};
+                
+    % dynamic allocator
+    [params.sys_An, params.W_An, params.N, params.N_An] = annihilator(params.sys_Pbar);        
+    
+    % get denominator and set numbers of parameters
+    [~, D] = tfdata(params.W_An(1,1));
+    params.NumPsi = length(D{1});
+    
+    % params to be estimated
+    for i=1:params.NumPsi
+       tmpstr =  ['params.psi_', num2str(i) ' = D{1}(end-i+1);'];
+       eval(tmpstr);       
+    end
+    params.Psi = reshape(D{1},params.NumPsi,1);  
+    
+    % static allocator
+    params.Pstar = dcgain(params.sys_P);
+    params.Pstar_tilde = dcgain(params.sys_Ptilde);
+    params.Pstar_bar = dcgain(params.sys_Pbar);
+    params.Pperp_bar = null(params.Pstar_bar);
+    
+    % input allocation (Zaccarian)
+    params.Bperp = null([params.B; params.D]);
+    
+    % dynamic output annihilator
+    params.Anstar = dcgain(params.sys_An);
+    
+    % optimizer (Sigma_op)
+    if params.Zaccarian
+        params.dim_state_op = size(params.Bperp,2); 
+        params.W = eye(params.dim_input);
+    else
+        params.dim_state_op = size(params.Pperp_bar,2); 
+    end            
+    
+    if params.staticAll
+        % annihilator (Sigma_an)
+        params.dim_state_an = 1;
+        params.A_an = zeros(params.dim_state_an);
+        params.B_an = zeros(params.dim_state_an,params.dim_state_op);
+        params.C_an = zeros(params.dim_state_an);
+        params.D_an = params.Pperp_bar;
+        if params.Zaccarian
+            params.Rzac = 1e1*eye(params.dim_state_op);   
+            params.W = eye(params.dim_input);
+            params.D_an_zac = params.Bperp;
+        end
+        params.R = 1e1*eye(params.eta);
+        params.gamma = -1e-1;
+    else
+        params.A_an = params.sys_An.A;
+        params.B_an = params.sys_An.B;
+        params.C_an = params.sys_An.C;
+        params.D_an = params.sys_An.D;
+        params.dim_state_an = size(params.A_an,1);
+        params.R = 1e2*eye(params.eta);
+        params.gamma = -1e-1;
+    end
+            
+    % state dimension
+    params.dim_state = params.n + params.dim_state_c + params.dim_state_r + params.dim_state_op + params.dim_state_an + params.NumPsi;        
+    
+    % initial condition
+    % [x, xc, xop, xan, psi]
+    params.X(1).val(:,1) = [ones(params.dim_state_r,1); zeros(params.dim_state_c,1); zeros(params.dim_state_op,1); zeros(params.dim_state_an,1); 0*ones(params.n,1); params.Psi];
+    
+    % position in the state vector of the estimated parameters
+    params.estimated_params = [params.n + params.dim_state_c + params.dim_state_r + params.dim_state_op + params.dim_state_an + 1:params.dim_state];
+    
+    % which vars am I optimising
+    params.opt_vars = [params.n + params.dim_state_c + params.dim_state_r + params.dim_state_op + params.dim_state_an + 1:params.dim_state];
+    
+    % set the not optimised vars
+    tmp = 1:length(params.X(1).val(:,1));
+    tmp_idx = tmp;
+    for i=1:length(params.opt_vars)
+        tmp_idx = intersect(tmp_idx,find(tmp~=params.opt_vars(i)));
+    end
+    params.nonopt_vars = tmp_idx;
+    
+    % plot vars (used to plot the state estimation. When the parameters are
+    % too many, consider to use only the true state components)
+    params.plot_vars = 1:(params.n + params.dim_state_c + params.dim_state_r + params.dim_state_op + params.dim_state_an);
+    params.plot_params = (params.n + params.dim_state_c + params.dim_state_r + params.dim_state_op + params.dim_state_an + 1):params.dim_state;   
+    
+    % number of reference trajectories (under development)
+    params.Ntraj = 1;
+    
+    % perturbed models
+    params.sys_pert(1).A = params.A;
+    params.sys_pert(1).B = params.B;
+    params.sys_pert(1).C = params.C;
+    params.sys_pert(1).D = params.D;
+    params.sys_pert(1).sys = params.sys_P;
+    
+    % pert perc
+    params.pert_perc = 0.05;
+    for i=2:params.Ntraj
+        params.sys_pert(i).A = params.A.*(1+params.pert_perc*randn(size(params.A)));
+        params.sys_pert(i).B = params.B.*(1+params.pert_perc*randn(size(params.B)));
+        params.sys_pert(i).C = params.C;
+        params.sys_pert(i).D = params.D;
+        params.sys_pert(i).sys = ss(params.sys_pert(i).A,params.sys_pert(i).B,params.sys_pert(i).C,params.sys_pert(i).D);
+        
+        params.X(i).val(:,1) = params.X(i-1).val(:,1);
+    end
+end
\ No newline at end of file
diff --git a/Lib/models/TCV/General/params_update_TCV.m b/Lib/models/TCV/General/params_update_TCV.m
new file mode 100644
index 0000000..c0dcc8b
--- /dev/null
+++ b/Lib/models/TCV/General/params_update_TCV.m
@@ -0,0 +1,24 @@
+%% PARAMS_UPDATE_OSCILLATOR_VDP
+% file: params_update_control_test.m
+% author: Federico Oliva
+% date: 22/06/2022
+% description: this function updates the estimated parameters on an
+% unstable LTI model
+% INPUT:
+% params: structure with all the necessary parameters 
+% x: state vector
+% OUTPUT:
+% params_out: updated structure with the new model parameters
+function params_out = params_update_TCV(params,x)
+
+    % assign params
+    params_out = params;    
+
+    % update psi
+    for i=1:params.NumPsi
+        tmpstr =  ['params_out.psi_', num2str(i) ' = x(params.n+', num2str(i), ');'];
+        eval(tmpstr); 
+    end
+
+    
+end
\ No newline at end of file
diff --git a/Lib/models/TCV/General_Discrete/model_TCV_Zaccarian_outAll_discrete.m b/Lib/models/TCV/General_Discrete/model_TCV_Zaccarian_outAll_discrete.m
new file mode 100644
index 0000000..4fa6644
--- /dev/null
+++ b/Lib/models/TCV/General_Discrete/model_TCV_Zaccarian_outAll_discrete.m
@@ -0,0 +1,76 @@
+%% MODEL_OSCILLATOR_VDP
+% file: model_control_test.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function describes the dynamics equation of an unstable
+% LTI model
+% INPUT:
+% t: time instant
+% x: state vector
+% params: structure with all the necessary parameters 
+% obs: observer class instance (may be not used)
+% OUTPUT:
+% x_dot: dynamics equations
+function x_dot = model_TCV_Zaccarian_outAll_discrete(t,x,params,obs)
+
+    % init the dynamics 
+    x_dot = zeros(length(x),1);
+    
+    % compute the time index
+    tdiff = obs.setup.time-t;   
+    pos = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+    pos = max(1,min(pos,size(obs.init.Y_full_story(obs.init.traj).val,3)));
+    
+    % compute y out of p
+    y = obs.setup.measure(x,params,t,obs.init.input_story(obs.init.traj).val(:,max(1,pos-1)));
+    
+    % compute the reference (Sigma_r)
+    range = 1:params.dim_state_r;
+    x_dot(range,:) = params.Ar*x(range,:) + params.Br*0; 
+    r = params.Cr*x(range,:) + params.Dr*0;    
+    
+    % compute the control (Sigma_c)
+    range = params.dim_state_r+1:params.dim_state_r+params.dim_state_c;
+    rbar = r(params.q_pos);
+    ybar = y(params.q_pos);    
+    e = ybar;
+    obs.init.error_story_ref(obs.init.traj).val(:,pos) = e;
+    x_dot(range,:) = params.Ac*x(range,:) + params.Bc*e + params.Bcr*rbar; 
+    u = params.Cc*x(range,:) + params.Dc*e + params.Dcr*rbar;           
+    obs.init.input_default_story(obs.init.traj).val(:,pos) = u;  
+    
+    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+    %%%%%%%%%%%% ALLOCATION %%%%%%%%%%%
+    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+    
+    %%%%%% compute the optimizer %%%%%%
+    range = params.dim_state_r+params.dim_state_c+1:params.dim_state_r+params.dim_state_c+params.dim_state_op;         
+    % gradient    
+    % static grad
+    ytilde = y(params.eta_pos);
+    grad = transpose(params.Pperp_bar)*transpose(params.Pstar_tilde)*params.R*params.Pstar_tilde;
+    x_dot(range,:) = params.gamma*(grad*params.Pperp_bar*x(range,:) + grad*u);      
+    v = x(range,:);
+    obs.init.optimizer_story(obs.init.traj).val(:,pos) = v;
+    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
+    
+    %%%%%% compute the annihilator %%%%%%
+    range = params.dim_state_r+params.dim_state_c+params.dim_state_op+1:params.dim_state_r+params.dim_state_c+params.dim_state_op+params.dim_state_an;
+    x_dot(range,:) = params.A_an*x(range,:) + params.B_an*v;    
+    ya = params.C_an*x(range,:) + params.D_an*v;
+    obs.init.allocator_story(obs.init.traj).val(:,pos) = ya;
+        
+    % sum input
+%     u_new = u;
+    u_new = u+ya;
+    obs.init.input_story(obs.init.traj).val(:,pos) = u_new;  
+    
+    % model dynamics  (Sigma p)
+    range = params.dim_state_r+params.dim_state_c+params.dim_state_op+params.dim_state_an+1:params.dim_state_r+params.dim_state_c+params.dim_state_op+params.dim_state_an+params.n;
+    x_dot(range,:) = params.sys_pert(obs.init.traj).A*x(range,:) + params.sys_pert(obs.init.traj).B*(u_new);  
+    
+    if params.discrete
+        x_dot = x + x_dot;
+    end
+       
+end
\ No newline at end of file
diff --git a/Lib/models/TCV/General_Discrete/model_TCV_reference_Zaccarian_outAll_discrete.m b/Lib/models/TCV/General_Discrete/model_TCV_reference_Zaccarian_outAll_discrete.m
new file mode 100644
index 0000000..f9dd12e
--- /dev/null
+++ b/Lib/models/TCV/General_Discrete/model_TCV_reference_Zaccarian_outAll_discrete.m
@@ -0,0 +1,58 @@
+%% MODEL_OSCILLATOR_VDP
+% file: model_control_test.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function describes the dynamics equation of an unstable
+% LTI model
+% INPUT:
+% t: time instant
+% x: state vector
+% params: structure with all the necessary parameters 
+% obs: observer class instance (may be not used)
+% OUTPUT:
+% x_dot: dynamics equations
+function x_dot = model_TCV_reference_Zaccarian_outAll_discrete(t,x,params,obs)
+
+    % init the dynamics     
+    if params.discrete == 0
+        x_dot = zeros(length(x),1);
+    else
+        x_dot = x;
+    end
+    
+    % compute the time index
+    tdiff = obs.setup.time-t;   
+    pos = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+    pos = max(1,min(pos,size(obs.init.Y_full_story(obs.init.traj).val,3)));
+    
+    % compute y out of p
+    y = obs.setup.measure_reference(x,params,t,obs.init.input_story_ref(obs.init.traj).val(:,max(1,pos-1)));
+    
+    % compute the reference (Sigma_r)
+    range = 1:params.dim_state_r;
+    x_dot(range,:) = params.discrete*x_dot(range,:) + params.Ar*x(range,:) + params.Br*0; 
+    r = params.Cr*x(range,:) + params.Dr*0;
+    obs.init.reference_story(obs.init.traj).val(:,pos) = r;
+    
+    % compute the control (Sigma_c)
+    range = params.dim_state_r+1:params.dim_state_r+params.dim_state_c;
+    rbar = r(params.q_pos);
+    ybar = y(params.q_pos);    
+    e = ybar;
+    obs.init.error_story_ref(obs.init.traj).val(:,pos) = e;
+    x_dot(range,:) = params.discrete*x_dot(range,:) + params.Ac*x(range,:) + params.Bc*e + params.Bcr*rbar; 
+    u = params.Cc*x(range,:) + params.Dc*e + params.Dcr*rbar;            
+    obs.init.input_story_ref(obs.init.traj).val(:,pos) = u;        
+    
+    % model dynamics  (Sigma p)
+    range = params.dim_state_r+params.dim_state_c+params.dim_state_op+params.dim_state_an+1:params.dim_state_r+params.dim_state_c+params.dim_state_op+params.dim_state_an+params.n;
+    x_dot(range,:) = params.discrete*x_dot(range,:) + params.sys_pert(obs.init.traj).A*x(range,:) + params.sys_pert(obs.init.traj).B*(u);  
+    
+    
+    %%% only for safety %%%
+    if strcmp(func2str(obs.setup.model),func2str(obs.setup.model_reference))
+        obs.init.input_default_story(obs.init.traj).val(:,pos) = u;
+        obs.init.input_story(obs.init.traj).val(:,pos) = u;
+    end
+ 
+end
\ No newline at end of file
diff --git a/Lib/models/TCV/General_Discrete/params_TCV_Zaccarian_outAll_discrete.m b/Lib/models/TCV/General_Discrete/params_TCV_Zaccarian_outAll_discrete.m
new file mode 100644
index 0000000..88dfc6f
--- /dev/null
+++ b/Lib/models/TCV/General_Discrete/params_TCV_Zaccarian_outAll_discrete.m
@@ -0,0 +1,218 @@
+    %% PARAMS_OSCILLATOR_VDP
+% file: params_control_test.m
+% author: Federico Oliva
+% date: 22/06/2022
+% description: this function initialises the parameters for an unstable LTI
+% model
+% INPUT: none
+% OUTPUT:
+% params: structure with all the necessary parameters
+function params = params_TCV_Zaccarian_outAll_discrete(varargin)
+
+    params.discrete = varargin{1}{4};
+    Ts = varargin{1}{5};
+    
+    % Zaccarian
+    % generate system
+    params.A = [-0.157 -0.094; ...
+                -0.416 -0.45];
+    params.B = [0.87 0.253 0.743; ...
+                0.39 0.354 0.65];
+    params.C = [0 1; 1 0];
+    params.D = [0 0 0; 0 0 0];
+    params.sys_P = ss(params.A,params.B,params.C,params.D); 
+    if params.discrete == 1
+        params.sys_P_CT = params.sys_P;
+        params.A_CT = params.A;
+        params.B_CT = params.B;
+        params.C_CT = params.C;
+        params.D_CT = params.D;
+        
+        params.sys_P = c2d(params.sys_P,Ts);        
+        params.A = params.sys_P.A;
+        params.B = params.sys_P.B;
+        params.C = params.sys_P.C;
+        params.D = params.sys_P.D;
+    end
+
+    % generate dimensions
+    params.n = size(params.A,1);
+    params.m = size(params.B,2);
+    params.p = size(params.C,1);
+
+    % generate subsystem bar
+    params.q_pos = 1;    
+    params.q = length(params.q_pos); 
+    params.Cbar = params.C(params.q_pos,:);
+    params.Dbar = params.D(params.q_pos,:);    
+    params.sys_Pbar = ss(params.A,params.B,params.Cbar,params.Dbar);    
+    if params.discrete == 1        
+        params.Cbar_CT = params.C_CT(params.q_pos,:);
+        params.Dbar_CT = params.D_CT(params.q_pos,:);
+        params.sys_Pbar_CT = ss(params.A_CT,params.B_CT,params.Cbar_CT,params.Dbar_CT);
+        params.sys_Pbar = c2d(params.sys_Pbar,Ts);      
+    end
+    
+    % generate subsystem tilde        
+    params.eta_pos = 1:params.p;
+    params.eta_pos(params.q_pos) = [];
+    params.eta = params.p-params.q;
+    params.Ctilde = params.C(params.eta_pos,:);        
+    params.Dtilde = params.D(params.eta_pos,:);
+    params.sys_Ptilde = ss(params.A,params.B,params.Ctilde,params.Dtilde);
+    if params.discrete == 1
+        params.sys_Ptilde = c2d(params.sys_Ptilde,Ts);         
+    end
+    
+    % reference signal system (Sigma_r)
+    params.dim_state_r = size(params.C,1);
+    params.Ar = zeros(params.dim_state_r);
+    params.Br = 0;
+    if params.discrete == 0
+        params.Cr = eye(params.dim_state_r);
+    else
+        params.Cr = eye(params.dim_state_r);
+    end
+    params.Dr = 0;    
+                    
+    % input stuff
+    params.dim_input = params.m;    
+    
+    % controller system (Sigma_c)    
+    params.dim_state_c = 4;
+    params.Ac = [-1.57 0.5767 0.822 -0.65;...
+                 -0.9 -0.501 -0.94 0.802; ...
+                 0 1 -1.61 1.614; ...
+                 0 0 0 0];
+    params.Bcr = [0; 0; 0; 1];
+    params.Bc = -params.Bcr;
+    params.Cc = [1.81 -1.2 -0.46 0; ...
+                 -0.62 1.47 0.89 0; ...
+                 0 0 0 0];
+    params.Dcr = [0; 0; 0];
+    params.Dc = -params.Dcr;
+    params.sys_C = ss(params.Ac,[params.Bc params.Bcr],params.Cc,[params.Dc params.Dcr]); 
+    if params.discrete == 1
+        params.sys_C = c2d(params.sys_C,Ts); 
+        params.Ac = params.sys_C.A;
+        params.Bc = params.sys_C.B(:,1:size(params.Bc,2));
+        params.Bcr = params.sys_C.B(:,size(params.Bc,2)+1:end);
+        params.Cc = params.sys_C.C;
+        params.Dc = params.sys_C.D(:,1:size(params.Bc,2));
+        params.Dcr = params.sys_C.D(:,size(params.Bc,2)+1:end);
+    end
+    
+    % choose allocator
+    params.staticAll = varargin{1}{3};
+    
+    % dynamic annihilator
+    if params.discrete == 0
+        [params.sys_An, params.W_An, params.N, params.N_An, params.num_An] = annihilator(params.sys_Pbar);            
+    else
+        [params.sys_An, params.W_An, params.N, params.N_An, params.num_An] = annihilator(params.sys_Pbar_CT);            
+    end
+    % get denominator and set numbers of parameters
+    [~, D] = tfdata(params.W_An(1,1));
+    params.NumPsi = length(D{1});    
+    % params to be estimated
+    for i=1:params.NumPsi
+       tmpstr =  ['params.psi_', num2str(i) ' = D{1}(end-i+1);'];
+       eval(tmpstr);       
+    end
+    params.Psi = reshape(D{1},params.NumPsi,1);
+    % custom
+    params.Psi = [2.7225e-01 1.4015e+01 1.0018e+03 4.1827e+04]';
+    
+    % output allocation         
+    params.Pstar = dcgain(params.sys_P);
+    params.Pstar_tilde = dcgain(params.sys_Ptilde);
+    params.Pstar_bar = dcgain(params.sys_Pbar);
+    
+    if params.staticAll
+        params.Pperp_bar = null(params.Pstar_bar);
+    else
+        params.Pperp_bar = dcgain(params.sys_An);
+    end
+    
+    % optimizer (Sigma_op)    
+    params.dim_state_op = size(params.Pperp_bar,2);          
+    
+    % annihilator (Sigma_an)
+    if params.staticAll
+        params.dim_state_an = 1;
+        params.A_an = zeros(params.dim_state_an);
+        params.B_an = zeros(params.dim_state_an,params.dim_state_op);
+        params.C_an = zeros(params.dim_state_an);                          
+        params.D_an = params.Pperp_bar;         
+        params.sys_An = ss(params.A_an,params.B_an,params.C_an,params.D_an);
+    else
+        params.A_an = params.sys_An.A;
+        params.B_an = params.sys_An.B;
+        params.C_an = params.sys_An.C;
+        params.D_an = params.sys_An.D;        
+        params.dim_state_an = size(params.A_an,1);
+    end   
+    if params.discrete == 1
+        params.sys_An = c2d(params.sys_An,Ts);
+        params.A_an = params.sys_An.A;
+        params.B_an = params.sys_An.B;
+        params.C_an = params.sys_An.C;
+        params.D_an = params.sys_An.D;  
+    end
+    params.Rcoef = 1e10;
+    params.R = diag(params.Rcoef)*eye(params.eta); 
+    params.gamma = -1e-2;
+            
+    % state dimension
+    params.dim_state = params.n + params.dim_state_c + params.dim_state_r + params.dim_state_op + params.dim_state_an + params.NumPsi; % + length(params.gamma) + length(params.Rcoef);
+    
+    % initial condition
+    % [x, xc, xop, xan, psi]
+    params.X(1).val(:,1) = [ones(params.dim_state_r,1); zeros(params.dim_state_c,1); zeros(params.dim_state_op,1); zeros(params.dim_state_an,1); 0*ones(params.n,1); params.Psi];
+        %params.gamma; params.Rcoef; params.Psi];
+    
+    % position in the state vector of the estimated parameters
+    params.estimated_params = [params.n + params.dim_state_c + params.dim_state_r + params.dim_state_op + params.dim_state_an + 1:params.dim_state];
+    
+    % which vars am I optimising
+    params.opt_vars = [params.n + params.dim_state_c + params.dim_state_r + params.dim_state_op + params.dim_state_an + 1:params.dim_state];    
+    
+    % set the not optimised vars
+    tmp = 1:length(params.X(1).val(:,1));
+    tmp_idx = tmp;
+    for i=1:length(params.opt_vars)
+        tmp_idx = intersect(tmp_idx,find(tmp~=params.opt_vars(i)));
+    end
+    params.nonopt_vars = tmp_idx;
+    
+    % out vars
+    params.OutDim = 2;
+    params.OutDim_compare = [params.q_pos];    
+    
+    % plot vars (used to plot the state estimation. When the parameters are
+    % too many, consider to use only the true state components)
+    params.plot_vars = 1:(params.n + params.dim_state_c + params.dim_state_r + params.dim_state_op + params.dim_state_an);
+    params.plot_params = (params.n + params.dim_state_c + params.dim_state_r + params.dim_state_op + params.dim_state_an + 1):params.dim_state;   
+    
+    % number of reference trajectories (under development)
+    params.Ntraj = 20;
+    
+    % perturbed models
+    params.sys_pert(1).A = params.A;
+    params.sys_pert(1).B = params.B;
+    params.sys_pert(1).C = params.C;
+    params.sys_pert(1).D = params.D;
+    params.sys_pert(1).sys = params.sys_P;
+    
+    % pert perc
+    params.pert_perc = 0.05;
+    for i=2:params.Ntraj
+        params.sys_pert(i).A = params.A.*(1+params.pert_perc*randn(size(params.A)));
+        params.sys_pert(i).B = params.B.*(1+params.pert_perc*randn(size(params.B)));
+        params.sys_pert(i).C = params.C;
+        params.sys_pert(i).D = params.D;
+        params.sys_pert(i).sys = ss(params.sys_pert(i).A,params.sys_pert(i).B,params.sys_pert(i).C,params.sys_pert(i).D);        
+        
+        params.X(i).val(:,1) = params.X(1).val(:,1);
+    end
+end
\ No newline at end of file
diff --git a/Lib/models/TCV/General_Discrete/params_update_TCV_Zaccarian_outAll_discrete.m b/Lib/models/TCV/General_Discrete/params_update_TCV_Zaccarian_outAll_discrete.m
new file mode 100644
index 0000000..ead66b2
--- /dev/null
+++ b/Lib/models/TCV/General_Discrete/params_update_TCV_Zaccarian_outAll_discrete.m
@@ -0,0 +1,45 @@
+%% PARAMS_UPDATE_OSCILLATOR_VDP
+% file: params_update_control_test.m
+% author: Federico Oliva
+% date: 22/06/2022
+% description: this function updates the estimated parameters on an
+% unstable LTI model
+% INPUT:
+% params: structure with all the necessary parameters 
+% x: state vector
+% OUTPUT:
+% params_out: updated structure with the new model parameters
+function params_out = params_update_TCV_Zaccarian_outAll_discrete(params,x)
+
+    % assign params
+    params_out = params; 
+    
+    if ~params.staticAll
+        
+%         params_out.Rcoef = x(end+1-params.NumPsi-length(params.Rcoef));
+%         params_out.R = diag(params_out.Rcoef);
+%         params_out.gamma = x(end+1-params.NumPsi-length(params.gamma)-length(params.Rcoef));
+
+        % update psi
+        for i=1:params.NumPsi
+            tmpstr =  ['params_out.psi_', num2str(i) ' = x(end-', num2str(params.NumPsi-i), ');'];
+            eval(tmpstr); 
+        end
+
+        %%% update the denominator %%%
+        Psi = 'den = [';
+        for i=1:params.NumPsi
+            Psi = [Psi, 'params_out.psi_', num2str(i), ','];
+        end
+        Psi = [Psi(1:end-1), '];'];
+        eval(Psi);    
+
+        % update An
+        W_An = tf(params.num_An, den);
+        params_out.sys_An = ss(W_An);
+        params_out.A_an = params_out.sys_An.A;
+        params_out.B_an = params_out.sys_An.B;
+        params_out.C_an = params_out.sys_An.C;
+        params_out.D_an = params_out.sys_An.D;
+    end
+end
\ No newline at end of file
diff --git a/Lib/models/TCV/Jcost.m b/Lib/models/TCV/Jcost.m
new file mode 100644
index 0000000..8a69f60
--- /dev/null
+++ b/Lib/models/TCV/Jcost.m
@@ -0,0 +1,17 @@
+%%
+function J = Jcost(x,yc,params) 
+
+    % init
+    J = 0;    
+    Anstar = params.Anstar;  
+    range = params.dim_state_r+params.dim_state_c+1:params.dim_state_r+params.dim_state_c+params.dim_state_op;
+    xop = x(range);
+    u = yc + Anstar*xop;
+
+    for i=1:params.m
+        if abs(u(i))>params.InputBound            
+            J =  J + 0.5*params.R(i)*u(i)^2;            
+        end
+    end
+
+end
\ No newline at end of file
diff --git a/Lib/models/TCV/ZaccarianInput/model_TCV_Zaccarian.m b/Lib/models/TCV/ZaccarianInput/model_TCV_Zaccarian.m
new file mode 100644
index 0000000..885bd8d
--- /dev/null
+++ b/Lib/models/TCV/ZaccarianInput/model_TCV_Zaccarian.m
@@ -0,0 +1,82 @@
+%% MODEL_OSCILLATOR_VDP
+% file: model_control_test.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function describes the dynamics equation of an unstable
+% LTI model
+% INPUT:
+% t: time instant
+% x: state vector
+% params: structure with all the necessary parameters 
+% obs: observer class instance (may be not used)
+% OUTPUT:
+% x_dot: dynamics equations
+function x_dot = model_TCV_Zaccarian(tspan,x,params,obs)
+
+    % init the dynamics         
+    x_dot = repmat(x,1,max(1,length(tspan)-1));    
+    t = tspan(1);
+    
+    % compute the time index
+    for i=1:length(tspan)
+        tdiff = obs.setup.time-tspan(i);   
+        pos(i) = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+        pos(i) = max(1,pos(i));        
+    end
+    drive_out = [];
+    params.u = params.input(tspan,drive_out,params);   
+
+    % compute y out of p
+    y = obs.setup.measure(x,params,t,obs.init.input_story_ref(obs.init.traj).val(:,max(1,pos-1)));
+    
+    % compute the reference (Sigma_r)
+    range = 1:params.dim_state_r;
+    x_dot(range,:) = params.Ar*x(range,:) + params.Br*0; 
+    r = params.Cr*x(range,:) + params.Dr*params.u;   
+    
+    % shift tspan
+    tspan = tspan-tspan(1);    
+    
+    % compute the control (Sigma_c)
+    range = params.dim_state_r+1:params.dim_state_r+params.dim_state_c;
+    rbar = r(params.q_pos);
+    ybar = y(params.q_pos);    
+    obs.init.error_story_ref(obs.init.traj).val(:,pos) = ybar;
+    x_dot(range,:) = params.Ac*x(range,:) + params.Bc*ybar + params.Bcr*rbar; 
+    yc = params.Cc*x(range,:) + params.Dc*ybar + params.Dcr*rbar;           
+    obs.init.input_default_story(obs.init.traj).val(:,pos) = yc;  
+    
+    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+    %%%%%%%%%%%% ALLOCATION %%%%%%%%%%%
+    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+    
+    %%%%%% compute the optimizer %%%%%%
+    range = params.dim_state_r+params.dim_state_c+1:params.dim_state_r+params.dim_state_c+params.dim_state_op;             
+    x_dot(range,:) = params.A_op*x(range,:) + params.B_op*yc;
+    v = params.C_op*x(range,:) + params.D_op*yc;          
+    obs.init.optimizer_story(obs.init.traj).val(:,pos) = v;
+    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+    
+    %%%%%% compute the annihilator %%%%%%
+    range = params.dim_state_r+params.dim_state_c+params.dim_state_op+1:params.dim_state_r+params.dim_state_c+params.dim_state_op+params.dim_state_an;
+    x_dot(range,:) = params.A_an*x(range,:) + params.B_an*v;    
+    ya = params.C_an*x(range,:) + params.D_an*v;
+    obs.init.allocator_story(obs.init.traj).val(:,pos) = ya;
+        
+    % sum input
+    u = yc+ya;    
+    
+    % model dynamics  (Sigma p)
+    range = params.dim_state_r+params.dim_state_c+params.dim_state_op+params.dim_state_an+1:params.dim_state_r+params.dim_state_c+params.dim_state_op+params.dim_state_an+params.n;
+    x_dot(range,:) = params.sys_pert(obs.init.traj).A*x(range,:) + params.sys_pert(obs.init.traj).B*u;  
+
+    obs.init.input_story(obs.init.traj).val(:,pos) = u';  
+    obs.init.input_pos_vec = pos;
+    
+    % save error
+    rbar = r(:,params.q_pos);
+    ybar = y(:,params.q_pos);    
+    e = rbar-ybar;
+    obs.init.error_story_ref(obs.init.traj).val(:,pos) = e';
+       
+end
\ No newline at end of file
diff --git a/Lib/models/TCV/ZaccarianInput/model_TCV_Zaccarian_Lsim.m b/Lib/models/TCV/ZaccarianInput/model_TCV_Zaccarian_Lsim.m
new file mode 100644
index 0000000..caca660
--- /dev/null
+++ b/Lib/models/TCV/ZaccarianInput/model_TCV_Zaccarian_Lsim.m
@@ -0,0 +1,56 @@
+%% MODEL_OSCILLATOR_VDP
+% file: model_control_test.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function describes the dynamics equation of an unstable
+% LTI model
+% INPUT:
+% t: time instant
+% x: state vector
+% params: structure with all the necessary parameters 
+% obs: observer class instance (may be not used)
+% OUTPUT:
+% x_dot: dynamics equations
+function x_dot = model_TCV_Zaccarian_Lsim(tspan,x,params,obs)
+
+    % init the dynamics         
+    x_dot = repmat(x,1,max(1,max(1,length(tspan)-1)));    
+    t = tspan(1);
+    
+    % compute the time index
+    for i=1:length(tspan)
+        tdiff = obs.setup.time-tspan(i);   
+        pos(i) = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+        pos(i) = max(1,pos(i));        
+    end
+    drive_out = [];
+    params.u = params.input(tspan,drive_out,params);   
+    
+    
+    % compute the reference (Sigma_r)
+    range = 1:params.dim_state_r;
+    x_dot(range,:) = params.Ar*x(range,:) + params.Br*0; 
+    r = params.Cr*x(range,:) + params.Dr*params.u;   
+    
+    % shift tspan
+    tspan = tspan-tspan(1);
+    
+    % compute the CL plant
+    range = params.dim_state_r+1:params.dim_state - params.NumPsi - params.NumGamma;    
+    r_lsim = r;
+    x0 = x(range);        
+    [y,~,xout] = lsim(params.sys_pert(obs.init.traj).sys_CL_All,r_lsim,tspan,x0,'foh');
+    x_dot(range,:) = xout(2:end,:)';
+    
+     % save input story     
+    [u,~,~] = lsim(params.sys_pert(obs.init.traj).sys_CL_Allu,r_lsim,tspan,x0,'foh');  
+    obs.init.input_story(obs.init.traj).val(:,pos) = u';  
+    obs.init.input_pos_vec = pos;
+    
+    % save error
+    rbar = r(:,params.q_pos);
+    ybar = y(:,params.q_pos);    
+    e = rbar-ybar;
+    obs.init.error_story_ref(obs.init.traj).val(:,pos) = e';
+       
+end
\ No newline at end of file
diff --git a/Lib/models/TCV/ZaccarianInput/model_TCV_Zaccarian_Nonlinear.m b/Lib/models/TCV/ZaccarianInput/model_TCV_Zaccarian_Nonlinear.m
new file mode 100644
index 0000000..2e23cb2
--- /dev/null
+++ b/Lib/models/TCV/ZaccarianInput/model_TCV_Zaccarian_Nonlinear.m
@@ -0,0 +1,85 @@
+%% MODEL_OSCILLATOR_VDP
+% file: model_control_test.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function describes the dynamics equation of an unstable
+% LTI model
+% INPUT:
+% t: time instant
+% x: state vector
+% params: structure with all the necessary parameters 
+% obs: observer class instance (may be not used)
+% OUTPUT:
+% x_dot: dynamics equations
+function x_dot = model_TCV_Zaccarian_Nonlinear(tspan,x,params,obs)
+
+    % init the dynamics         
+    x_dot = repmat(x,1,max(1,length(tspan)-1));    
+    t = tspan(1);
+    
+    % compute the time index
+    for i=1:length(tspan)
+        tdiff = obs.setup.time-tspan(i);   
+        pos(i) = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+        pos(i) = max(1,pos(i));        
+    end
+    drive_out = [];
+    params.u = params.input(tspan,drive_out,params);   
+
+    % compute y out of p
+    y = obs.setup.measure(x,params,t,obs.init.input_story_ref(obs.init.traj).val(:,max(1,pos-1)));
+    
+    % compute the reference (Sigma_r)
+    range = 1:params.dim_state_r;
+    x_dot(range,:) = params.Ar*x(range,:) + params.Br*0; 
+    r = params.Cr*x(range,:) + params.Dr*params.u;   
+    
+    % shift tspan
+    tspan = tspan-tspan(1);    
+    
+    % compute the control (Sigma_c)
+    range = params.dim_state_r+1:params.dim_state_r+params.dim_state_c;
+    rbar = r(params.q_pos);
+    ybar = y(params.q_pos);    
+    obs.init.error_story_ref(obs.init.traj).val(:,pos) = ybar;
+    x_dot(range,:) = params.Ac*x(range,:) + params.Bc*ybar + params.Bcr*rbar; 
+    yc = params.Cc*x(range,:) + params.Dc*ybar + params.Dcr*rbar;           
+    obs.init.input_default_story(obs.init.traj).val(:,pos) = yc;  
+    
+    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+    %%%%%%%%%%%% ALLOCATION %%%%%%%%%%%
+    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+    
+    %%%%%% compute the optimizer %%%%%%
+    range = params.dim_state_r+params.dim_state_c+1:params.dim_state_r+params.dim_state_c+params.dim_state_op;      
+    u_prev = obs.init.input_story(obs.init.traj).val(:,max(1,pos-1));
+    x_dot(range,:) = DotXopt(x,yc,params);
+    v = x(range,:);
+    obs.init.optimizer_story(obs.init.traj).val(:,pos) = v;
+    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+    
+    %%%%%% compute the annihilator %%%%%%
+    range = params.dim_state_r+params.dim_state_c+params.dim_state_op+1:params.dim_state_r+params.dim_state_c+params.dim_state_op+params.dim_state_an;
+    x_dot(range,:) = params.A_an*x(range,:) + params.B_an*v;    
+    ya = params.C_an*x(range,:) + params.D_an*v;
+    obs.init.allocator_story(obs.init.traj).val(:,pos) = ya;
+        
+    % sum input
+    u = yc+ya;    
+    
+    % model dynamics  (Sigma p)
+    range = params.dim_state_r+params.dim_state_c+params.dim_state_op+params.dim_state_an+1:params.dim_state_r+params.dim_state_c+params.dim_state_op+params.dim_state_an+params.n;
+    x_dot(range,:) = params.sys_pert(obs.init.traj).A*x(range,:) + params.sys_pert(obs.init.traj).B*u;  
+
+    obs.init.input_story(obs.init.traj).val(:,pos) = u';  
+    obs.init.input_pos_vec = pos;
+    
+    % save error
+    rbar = r(:,params.q_pos);
+    ybar = y(:,params.q_pos);    
+    e = rbar-ybar;
+    obs.init.error_story_ref(obs.init.traj).val(:,pos) = e';
+    
+%     obs.init.Jstory_all(obs.init.traj).val(pos) = Jcost(x,u_prev,params); 
+       
+end
\ No newline at end of file
diff --git a/Lib/models/TCV/ZaccarianInput/model_TCV_reference_Zaccarian.m b/Lib/models/TCV/ZaccarianInput/model_TCV_reference_Zaccarian.m
new file mode 100644
index 0000000..bde2716
--- /dev/null
+++ b/Lib/models/TCV/ZaccarianInput/model_TCV_reference_Zaccarian.m
@@ -0,0 +1,53 @@
+%% MODEL_OSCILLATOR_VDP
+% file: model_control_test.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function describes the dynamics equation of an unstable
+% LTI model
+% INPUT:
+% t: time instant
+% x: state vector
+% params: structure with all the necessary parameters 
+% obs: observer class instance (may be not used)
+% OUTPUT:
+% x_dot: dynamics equations
+function x_dot = model_TCV_reference_Zaccarian(t,x,params,obs)
+
+    % init the dynamics 
+    x_dot = zeros(length(x),1);
+    
+    % compute the time index
+    tdiff = obs.setup.time-t;   
+    pos = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+    pos = max(1,min(pos,size(obs.init.Y_full_story(obs.init.traj).val,3)));
+    
+    % compute y out of p
+    y = obs.setup.measure_reference(x,params,t,obs.init.input_story_ref(obs.init.traj).val(:,max(1,pos-1)));
+    
+    % compute the reference (Sigma_r)
+    range = 1:params.dim_state_r;
+    x_dot(range,:) = params.Ar*x(range,:) + params.Br*0; 
+    r = params.Cr*x(range,:) + params.Dr*0;
+    obs.init.reference_story(obs.init.traj).val(:,pos) = r;
+    
+    % compute the control (Sigma_c)
+    range = params.dim_state_r+1:params.dim_state_r+params.dim_state_c;
+    rbar = r(params.q_pos);
+    ybar = y(params.q_pos);    
+    e = ybar;
+    obs.init.error_story_ref(obs.init.traj).val(:,pos) = e;
+    x_dot(range,:) = params.Ac*x(range,:) + params.Bc*e + params.Bcr*rbar; 
+    u = params.Cc*x(range,:) + params.Dc*e + params.Dcr*rbar;            
+    obs.init.input_story_ref(obs.init.traj).val(:,pos) = u;        
+    
+    % model dynamics  (Sigma p)
+    range = params.dim_state_r+params.dim_state_c+params.dim_state_op+params.dim_state_an+1:params.dim_state_r+params.dim_state_c+params.dim_state_op+params.dim_state_an+params.n;
+    x_dot(range,:) = params.A*x(range,:) + params.B*(u);   
+    
+    %%% only for safety %%%
+    if strcmp(func2str(obs.setup.model),func2str(obs.setup.model_reference))
+        obs.init.input_default_story(obs.init.traj).val(:,pos) = u;
+        obs.init.input_story(obs.init.traj).val(:,pos) = u;
+    end
+ 
+end
\ No newline at end of file
diff --git a/Lib/models/TCV/ZaccarianInput/params_TCV_Zaccarian.m b/Lib/models/TCV/ZaccarianInput/params_TCV_Zaccarian.m
new file mode 100644
index 0000000..1f8cd65
--- /dev/null
+++ b/Lib/models/TCV/ZaccarianInput/params_TCV_Zaccarian.m
@@ -0,0 +1,296 @@
+%% PARAMS_OSCILLATOR_VDP
+% file: params_control_test.m
+% author: Federico Oliva
+% date: 22/06/2022
+% description: this function initialises the parameters for an unstable LTI
+% model
+% INPUT: none
+% OUTPUT:
+% params: structure with all the necessary parameters
+function params = params_TCV_Zaccarian(varargin)
+
+    % case 1 = strong
+    % case 2 = static annihilator
+    % case 3 = dyn annihilator
+    params.case = 3;
+    
+    % Zaccarian
+    % generate system
+    params.A_def = [-0.157 -0.094; ...
+                -0.416 -0.45];
+    params.B_def = [0.87 0.253 0.743; ...
+                0.39 0.354 0.65];
+    params.C_def = [0 1];
+    params.D_def = [0 0 0];
+    params.sys_P_def = ss(params.A_def,params.B_def,params.C_def,params.D_def);
+    
+    % generate dimensions
+    params.n = size(params.A_def,1);
+    params.m = size(params.B_def,2);
+    params.p = size(params.C_def,1);
+    
+    % companion form
+    params.A = [-0.607, 1;
+                      -0.03155, 0];
+    params.B = [0.39 0.354 0.65;
+                      -0.3007 -0.04967 -0.207];
+    params.C = [1 0];
+    params.D = [0 0 0]; 
+    params.sys_P = ss(params.A, params.B, params.C, params.D);       
+
+    % generate subsystem bar
+    params.q_pos = 1;    
+    params.q = length(params.q_pos); 
+    params.eta = [];
+    
+    % reference signal system (Sigma_r)
+    params.dim_state_r = size(params.C,1);
+    params.Ar = zeros(params.dim_state_r);
+    params.Br = 0;
+    params.Cr = zeros(params.dim_state_r);
+    params.Dr = 1;   
+    params.sys_R = ss(params.Ar,params.Br,params.Cr,params.Dr);    
+    params.sys_R.InputName = 'ref';
+    params.sys_R.OutputName = 'r';
+    params.sys_Sum = sumblk('e = r - y',params.q);
+                    
+    % input stuff
+    params.dim_input = params.m;    
+    
+    % controller system (Sigma_c)    
+    params.dim_state_c = 4;
+    params.Ac = [-1.57 0.5767 0.822 -0.65;...
+                 -0.9 -0.501 -0.94 0.802; ...
+                 0 1 -1.61 1.614; ...
+                 0 0 0 0];
+    params.Bcr = [0; 0; 0; 1];
+    params.Bc = -params.Bcr;
+    params.Cc = [1.81 -1.2 -0.46 0; ...
+                 -0.62 1.47 0.89 0; ...
+                 0 0 0 0];
+    params.Dcr = [0; 0; 0];
+    params.Dc = -params.Dcr;
+    params.sys_C = ss(params.Ac,[params.Bc params.Bcr],params.Cc,[params.Dc params.Dcr]); 
+    
+    % only one input
+    params.sys_C_err = ss(params.Ac,params.Bcr,params.Cc,params.Dcr);   
+    params.sys_C_err.InputName = 'e';
+    params.sys_C_err.OutputName = 'yc';
+    
+    %%% define PSI    
+    Npoints = 5;
+    poles = -logspace(0,1,Npoints);
+
+    for i=1:Npoints-1 
+        params.POLES(i,:) = linspace(poles(i),poles(i+1),3);
+        params.PSI(i,:) = poly(params.POLES(i,:));
+        params.PSI(i,:) = params.PSI(i,:)./params.PSI(i,end);    
+    end       
+%     params.tpoints = CustomStartPointSet(params.PSI(:,1:end-1));
+    %%%%%%%%%%%%%%%%%%
+    
+    % optimizer (Sigma_op) 
+    if params.case == 1
+        % input allocation Strong (Zaccarian)
+        params.Bperp = null([params.B; params.D]);
+        params.dim_state_op = size(params.Bperp,2);
+        
+        % Sigma op
+        params.R = [1 1 1].*eye(params.dim_input);
+        params.gamma = 1e-1;
+        params.A_op = -params.gamma*params.Bperp'*params.R*params.Bperp;
+        params.B_op = -params.gamma*params.Bperp'*params.R;        
+        params.C_op = eye(params.dim_state_op);
+        params.D_op = zeros(params.dim_state_op,params.m);
+        params.sys_op = ss(params.A_op,params.B_op,params.C_op,params.D_op);
+        
+        % Sigma an
+        params.dim_state_an = 1;
+        params.A_an = zeros(params.dim_state_an);
+        params.B_an = zeros(params.dim_state_an,params.dim_state_op);
+        params.C_an = zeros(params.m,params.dim_state_an);
+        params.D_an = params.Bperp;
+        params.sys_An = ss(params.A_an,params.B_an,params.C_an,params.D_an);
+        
+        params.NumPsi = 0;
+        params.Psi = [];
+
+    elseif params.case == 2
+        % input allocation weak static (Zaccarian)  
+        params.Pstar = dcgain(params.sys_P);
+        params.Pperp = null(params.Pstar);        
+        params.dim_state_op = size(params.Pperp,2);
+        
+        % Sigma op
+        params.R = [1 1 1].*eye(params.dim_input);
+        params.gamma = 1e-1;
+        params.A_op = -params.gamma*params.Pperp'*params.R*params.Pperp;
+        params.B_op = -params.gamma*params.Pperp'*params.R;        
+        params.C_op = eye(params.dim_state_op);
+        params.D_op = zeros(params.dim_state_op,params.m);
+        params.sys_op = ss(params.A_op,params.B_op,params.C_op,params.D_op);
+        
+        % Sigma an
+        params.dim_state_an = 1;
+        params.A_an = zeros(params.dim_state_an);
+        params.B_an = zeros(params.dim_state_an,params.dim_state_op);
+        params.C_an = zeros(params.m,params.dim_state_an);
+        params.D_an = params.Pperp;
+        params.sys_An = ss(params.A_an,params.B_an,params.C_an,params.D_an);
+        
+        params.NumPsi = 0;
+        params.Psi = [];
+        
+    else
+        % input allocation weak dynamic (Zaccarian)  
+        % dynamic annihilator    
+        [params.num_An,params.N, params.N_An] = annihilator(params.sys_P, params.n + 1); 
+        W_An = tf(params.num_An, params.PSI(end,:));
+        % Compute the annihilator state-space model
+        params.sys_An = ss(W_An);
+        params.A_an = params.sys_An.A;
+        params.B_an = params.sys_An.B;
+        params.C_an = params.sys_An.C;
+        params.D_an = params.sys_An.D;
+        params.dim_state_an = size(params.A_an,1);
+        params.Psi = params.PSI(1,:)';
+%         params.Psi = [2.7118e+03   1.0795e+06   1.0781e+08   1.0000e+00]';
+        params.NumPsi = length(params.Psi);                            
+                
+        params.Anstar = dcgain(params.sys_An);        
+        params.dim_state_op = size(params.Anstar,2);
+        
+        % Sigma op
+        %%% first realisation (non minimal)
+        params.R = [1 1 1].*eye(params.dim_input);        
+        params.A_op_def = -params.Anstar'*params.R*params.Anstar;
+        params.B_op_def = -params.Anstar'*params.R;        
+        params.C_op_def = eye(params.dim_state_op);
+        params.D_op_def = zeros(params.dim_state_op,params.m);
+%         params.sys_op_def = minreal(ss(params.A_op_def,params.B_op_def,params.C_op_def,params.D_op_def),[],false);
+        params.sys_op_def = ss(params.A_op_def,params.B_op_def,params.C_op_def,params.D_op_def);
+        %%% minimal realisation (update with GAMMA)
+        params.dim_state_op = size(params.sys_op_def.A,2);
+        
+        %%% define GAMMA    
+        Npoints = 5;
+        decades = logspace(-2,1,Npoints);
+
+        for i=1:Npoints 
+            params.GAMMA(i,:) = repmat(decades(i),1,params.dim_state_op);            
+        end      
+        % define tpoints        
+        params.tpoints = CustomStartPointSet(params.GAMMA);
+        %%%%%%%%%%%%%%%%%%
+        
+%         params.gamma = params.GAMMA(1,:)';
+        params.gamma = 1.0*ones(params.dim_state_op,1);       
+%         params.gamma = 1e0*[1.9429e+02   4.1329e+02   1.4146e+00   1.1411e+00   8.5913e+01]';
+        params.Gamma = diag(params.gamma);
+        params.NumGamma = params.dim_state_op;
+        params.A_op = params.Gamma*params.sys_op_def.A;
+        params.B_op = params.Gamma*params.sys_op_def.B;
+        params.C_op = params.sys_op_def.C;
+        params.D_op = params.sys_op_def.D;
+        params.sys_op = ss(params.A_op,params.B_op,params.C_op,params.D_op);                    
+                
+        % get steady state input 
+        A_inf = [params.Ac, -params.Bc * params.C;
+                params.B*params.Cc, params.A];
+        B_inf = [params.Bc;
+                 zeros(params.n, params.p)];
+        C_inf = [params.Cc, zeros(params.m, params.n)];
+        y_c_inf = -C_inf * pinv(A_inf) * B_inf;
+        params.u_inf = (eye(params.m) - params.Anstar * pinv(params.Anstar' * params.R * params.Anstar) * params.Anstar' * params.R) * y_c_inf;
+        
+    end    
+    
+    params.sys_An.InputName = 'v';
+    params.sys_An.OutputName = 'ya';
+    params.sys_op.InputName = 'yc';
+    params.sys_op.OutputName = 'v';
+    
+                            
+    % state dimension
+    params.dim_state = params.dim_state_r + params.dim_state_c + params.dim_state_op + params.dim_state_an + params.n + params.NumPsi + params.NumGamma;        
+    
+    % initial condition
+    % [x, xc, xop, xan, psi]
+    params.X(1).val(:,1) = [zeros(params.dim_state_r,1); zeros(params.dim_state_c,1); zeros(params.dim_state_op,1); zeros(params.dim_state_an,1); 0*ones(params.n,1); params.Psi; params.gamma];
+    
+    % position in the state vector of the estimated parameters
+    params.estimated_params = [params.dim_state_r + params.dim_state_c + params.dim_state_op + params.dim_state_an + params.n + 1:params.dim_state];
+    
+    % which vars am I optimising
+    %%% PSI opt %%%
+    %params.opt_vars = [params.dim_state_r + params.dim_state_c + params.dim_state_op + params.dim_state_an + params.n + 1:params.dim_state-params.NumGamma - 1];
+    %%% GAMMA opt %%%
+    %params.opt_vars = [params.dim_state-params.NumGamma+1:params.dim_state];
+    %%% ALL OPT %%%
+    params.PsiPos = params.dim_state_r + params.dim_state_c + params.dim_state_op + params.dim_state_an + params.n + 1:params.dim_state-params.NumGamma;
+    params.GammaPos = params.dim_state-params.NumGamma+1:params.dim_state;
+    params.opt_vars = [params.PsiPos(1:end-1) params.GammaPos];
+    
+    % set the not optimised vars
+    tmp = 1:length(params.X(1).val(:,1));
+    tmp_idx = tmp;
+    for i=1:length(params.opt_vars)
+        tmp_idx = intersect(tmp_idx,find(tmp~=params.opt_vars(i)));
+    end
+    params.nonopt_vars = tmp_idx;
+    
+    % out vars
+    params.OutDim = 1;
+    params.OutDim_compare = [1];
+    
+    % plot vars (used to plot the state estimation. When the parameters are
+    % too many, consider to use only the true state components)
+    params.plot_vars = 1:(params.n + params.dim_state_c + params.dim_state_r + params.dim_state_op + params.dim_state_an);
+    params.plot_params = (params.n + params.dim_state_c + params.dim_state_r + params.dim_state_op + params.dim_state_an + 1):params.dim_state;   
+    
+    % number of reference trajectories (under development)
+    params.Ntraj = 2;
+    params.traj = 1;
+    params.optimising = 0;
+    params.Ru = 0.25e-1;
+    
+    % perturbed models
+    params.sys_pert(1).A = params.A;
+    params.sys_pert(1).B = params.B;
+    params.sys_pert(1).C = params.C;
+    params.sys_pert(1).D = params.D;
+    params.sys_pert(1).sys_P = params.sys_P;
+    
+    % pert perc
+    params.pert_perc = 0.1;
+    for i=2:params.Ntraj
+        %%% no change in Pstar
+%         params.sys_pert(i).A = [[params.A(1:end-1,1).*(1+params.pert_perc*randn(params.n-1,1)); params.A(end,1)] params.A(:,2:end)];
+%         params.sys_pert(i).B = [params.B(1:end-1,:).*(1+params.pert_perc*randn(params.n-1,params.m)); params.B(end, :)];
+        %%% chage Pstar
+        params.sys_pert(i).A = [params.A(1:end,1).*(1+params.pert_perc*rand(params.n,1)) params.A(:,2:end)];
+        params.sys_pert(i).B = params.B.*(1+params.pert_perc*rand(params.n,params.m));
+        params.sys_pert(i).C = params.C;
+        params.sys_pert(i).D = params.D;
+        params.sys_pert(i).sys_P = ss(params.sys_pert(i).A,params.sys_pert(i).B,params.sys_pert(i).C,params.sys_pert(i).D);
+        
+        params.X(i).val(:,1) = params.X(i-1).val(:,1);
+    end
+    
+    %%% closed loop system %%%
+    for i=1:params.Ntraj
+        %%% no allocation closed loop %%%
+        % plant and controller        
+        params.sys_pert(i).sys_P.InputName = 'u';
+        params.sys_pert(i).sys_P.OutputName = 'y';        
+        params.sys_SumAll = sumblk('u = yc',params.m);        
+        params.sys_pert(i).sys_CLu = connect(params.sys_Sum,params.sys_C_err,params.sys_SumAll,params.sys_pert(i).sys_P,'r','u');
+        params.sys_pert(i).sys_CL = connect(params.sys_Sum,params.sys_C_err,params.sys_SumAll,params.sys_pert(i).sys_P,'r','y');         
+        
+        params.sys_SumAll = sumblk('u = yc + ya',params.m);  
+        params.sys_pert(i).sys_CL_Allu = connect(params.sys_Sum,params.sys_C_err,params.sys_op,params.sys_An,params.sys_SumAll,params.sys_pert(i).sys_P,'r','u');
+        params.sys_pert(i).sys_CL_All = connect(params.sys_Sum,params.sys_C_err,params.sys_op,params.sys_An,params.sys_SumAll,params.sys_pert(i).sys_P,'r','y');
+        
+    end
+        
+end
\ No newline at end of file
diff --git a/Lib/models/TCV/ZaccarianInput/params_TCV_Zaccarian_Nonlinear.m b/Lib/models/TCV/ZaccarianInput/params_TCV_Zaccarian_Nonlinear.m
new file mode 100644
index 0000000..6240d57
--- /dev/null
+++ b/Lib/models/TCV/ZaccarianInput/params_TCV_Zaccarian_Nonlinear.m
@@ -0,0 +1,198 @@
+%% PARAMS_OSCILLATOR_VDP
+% file: params_control_test.m
+% author: Federico Oliva
+% date: 22/06/2022
+% description: this function initialises the parameters for an unstable LTI
+% model
+% INPUT: none
+% OUTPUT:
+% params: structure with all the necessary parameters
+function params = params_TCV_Zaccarian_Nonlinear(varargin)
+    
+    % Zaccarian
+    % generate system
+    params.A_def = [-0.157 -0.094; ...
+                -0.416 -0.45];
+    params.B_def = [0.87 0.253 0.743; ...
+                0.39 0.354 0.65];
+    params.C_def = [0 1];
+    params.D_def = [0 0 0];
+    params.sys_P_def = ss(params.A_def,params.B_def,params.C_def,params.D_def);
+    
+    % generate dimensions
+    params.n = size(params.A_def,1);
+    params.m = size(params.B_def,2);
+    params.p = size(params.C_def,1);
+    
+    % companion form
+    params.A = [-0.607, 1;
+                      -0.03155, 0];
+    params.B = [0.39 0.354 0.65;
+                      -0.3007 -0.04967 -0.207];
+    params.C = [1 0];
+    params.D = [0 0 0]; 
+    params.sys_P = ss(params.A, params.B, params.C, params.D);       
+
+    % generate subsystem bar
+    params.q_pos = 1;    
+    params.q = length(params.q_pos); 
+    params.eta = [];
+    
+    % reference signal system (Sigma_r)
+    params.dim_state_r = size(params.C,1);
+    params.Ar = zeros(params.dim_state_r);
+    params.Br = 0;
+    params.Cr = zeros(params.dim_state_r);
+    params.Dr = 1;   
+    params.sys_R = ss(params.Ar,params.Br,params.Cr,params.Dr);    
+                    
+    % input stuff
+    params.dim_input = params.m;    
+    
+    % controller system (Sigma_c)    
+    params.dim_state_c = 4;
+    params.Ac = [-1.57 0.5767 0.822 -0.65;...
+                 -0.9 -0.501 -0.94 0.802; ...
+                 0 1 -1.61 1.614; ...
+                 0 0 0 0];
+    params.Bcr = [0; 0; 0; 1];
+    params.Bc = -params.Bcr;
+    params.Cc = [1.81 -1.2 -0.46 0; ...
+                 -0.62 1.47 0.89 0; ...
+                 0 0 0 0];
+    params.Dcr = [0; 0; 0];
+    params.Dc = -params.Dcr;
+    params.sys_C = ss(params.Ac,[params.Bc params.Bcr],params.Cc,[params.Dc params.Dcr]); 
+    
+    %%% define PSI    
+    Npoints = 5;
+    poles = -logspace(0,1,Npoints);
+
+    for i=1:Npoints-1 
+        params.POLES(i,:) = linspace(poles(i),poles(i+1),3);
+        params.PSI(i,:) = poly(params.POLES(i,:));
+        params.PSI(i,:) = params.PSI(i,:)./params.PSI(i,end);    
+    end       
+%     params.tpoints = CustomStartPointSet(params.PSI(:,1:end-1));
+    %%%%%%%%%%%%%%%%%%
+    
+    % optimizer (Sigma_op) 
+    
+    % input allocation weak dynamic (Zaccarian)  
+    % dynamic annihilator    
+    [params.num_An, params.N, params.N_An] = annihilator(params.sys_P, params.n+1); 
+    W_An = tf(params.num_An, params.PSI(end,:));
+    % Compute the annihilator state-space model
+    params.sys_An = ss(W_An);
+    params.A_an = params.sys_An.A;
+    params.B_an = params.sys_An.B;
+    params.C_an = params.sys_An.C;
+    params.D_an = params.sys_An.D;
+    params.dim_state_an = size(params.A_an,1);
+    params.Psi = params.PSI(1,:)';
+    params.Psi = [0.1667    1.0000    1.8333    1.0000]';
+    params.NumPsi = length(params.Psi);                            
+            
+    params.Anstar = dcgain(params.sys_An);        
+    params.dim_state_op = size(params.Anstar,2);
+    
+    % Sigma op
+    %%% first realisation (non minimal)
+    params.R = [1 1 1];        
+    params.InputBound = 0.3;
+    %%% minimal realisation (update with GAMMA)
+    params.dim_state_op = size(params.Anstar,2);
+    
+    %%% define GAMMA    
+    Npoints = 5;
+    decades = logspace(-2,1,Npoints);
+
+    for i=1:Npoints 
+        params.GAMMA(i,:) = repmat(decades(i),1,params.dim_state_op);            
+    end      
+    % define tpoints        
+    params.tpoints = CustomStartPointSet(params.GAMMA);
+    %%%%%%%%%%%%%%%%%%
+    
+%         params.gamma = params.GAMMA(1,:)';
+    params.gamma = 1e0*ones(params.dim_state_op,1);       
+%     params.gamma = 1e0*[1.9429e+02   4.1329e+02   1.4146e+00   1.1411e+00   8.5913e+01]';
+    params.Gamma = diag(params.gamma);
+    params.NumGamma = params.dim_state_op;                  
+            
+    % get steady state input 
+    A_inf = [params.Ac, -params.Bc * params.C;
+            params.B*params.Cc, params.A];
+    B_inf = [params.Bc;
+             zeros(params.n, params.p)];
+    C_inf = [params.Cc, zeros(params.m, params.n)];
+    params.y_c_inf = -C_inf * pinv(A_inf) * B_inf;
+    params.u_inf = (eye(params.m) - params.Anstar * pinv(params.Anstar' * diag(params.R) * params.Anstar) * params.Anstar' * diag(params.R)) * params.y_c_inf;
+                                
+    % state dimension
+    params.dim_state = params.dim_state_r + params.dim_state_c + params.dim_state_op + params.dim_state_an + params.n + params.NumPsi + params.NumGamma;        
+    
+    % initial condition
+    % [x, xc, xop, xan, psi]
+    params.X(1).val(:,1) = [zeros(params.dim_state_r,1); zeros(params.dim_state_c,1); zeros(params.dim_state_op,1); zeros(params.dim_state_an,1); 0*ones(params.n,1); params.Psi; params.gamma];
+    
+    % position in the state vector of the estimated parameters
+    params.estimated_params = [params.dim_state_r + params.dim_state_c + params.dim_state_op + params.dim_state_an + params.n + 1:params.dim_state];
+    
+    % which vars am I optimising
+    %%% PSI opt %%%
+    %params.opt_vars = [params.dim_state_r + params.dim_state_c + params.dim_state_op + params.dim_state_an + params.n + 1:params.dim_state-params.NumGamma - 1];
+    %%% GAMMA opt %%%
+    %params.opt_vars = [params.dim_state-params.NumGamma+1:params.dim_state];
+    %%% ALL OPT %%%
+    params.PsiPos = params.dim_state_r + params.dim_state_c + params.dim_state_op + params.dim_state_an + params.n + 1:params.dim_state-params.NumGamma;
+    params.GammaPos = params.dim_state-params.NumGamma+1:params.dim_state;
+    params.opt_vars = [params.PsiPos(1:end-1) params.GammaPos];
+    
+    % set the not optimised vars
+    tmp = 1:length(params.X(1).val(:,1));
+    tmp_idx = tmp;
+    for i=1:length(params.opt_vars)
+        tmp_idx = intersect(tmp_idx,find(tmp~=params.opt_vars(i)));
+    end
+    params.nonopt_vars = tmp_idx;
+    
+    % out vars
+    params.OutDim = 1;
+    params.OutDim_compare = [1];
+    
+    % plot vars (used to plot the state estimation. When the parameters are
+    % too many, consider to use only the true state components)
+    params.plot_vars = 1:(params.n + params.dim_state_c + params.dim_state_r + params.dim_state_op + params.dim_state_an);
+    params.plot_params = (params.n + params.dim_state_c + params.dim_state_r + params.dim_state_op + params.dim_state_an + 1):params.dim_state;   
+    
+    % number of reference trajectories (under development)
+    params.Ntraj = 2;
+    params.traj = 1;
+    params.optimising = 0;
+    params.Ru = 1e-1;
+    
+    % perturbed models
+    params.sys_pert(1).A = params.A;
+    params.sys_pert(1).B = params.B;
+    params.sys_pert(1).C = params.C;
+    params.sys_pert(1).D = params.D;
+    params.sys_pert(1).sys_P = params.sys_P;
+    
+    % pert perc
+    params.pert_perc = 0.05;
+    for i=2:params.Ntraj
+        %%% no change in Pstar
+%         params.sys_pert(i).A = [[params.A(1:end-1,1).*(1+params.pert_perc*randn(params.n-1,1)); params.A(end,1)] params.A(:,2:end)];
+%         params.sys_pert(i).B = [params.B(1:end-1,:).*(1+params.pert_perc*randn(params.n-1,params.m)); params.B(end, :)];
+        %%% chage Pstar
+        params.sys_pert(i).A = [params.A(1:end,1).*(1+params.pert_perc*randn(params.n,1)) params.A(:,2:end)];
+        params.sys_pert(i).B = params.B.*(1+params.pert_perc*randn(params.n,params.m));
+        params.sys_pert(i).C = params.C;
+        params.sys_pert(i).D = params.D;
+        params.sys_pert(i).sys_P = ss(params.sys_pert(i).A,params.sys_pert(i).B,params.sys_pert(i).C,params.sys_pert(i).D);
+        
+        params.X(i).val(:,1) = params.X(i-1).val(:,1);
+    end       
+        
+end
\ No newline at end of file
diff --git a/Lib/models/TCV/ZaccarianInput/params_update_TCV_Zaccarian.m b/Lib/models/TCV/ZaccarianInput/params_update_TCV_Zaccarian.m
new file mode 100644
index 0000000..dcc7a42
--- /dev/null
+++ b/Lib/models/TCV/ZaccarianInput/params_update_TCV_Zaccarian.m
@@ -0,0 +1,52 @@
+%% PARAMS_UPDATE_OSCILLATOR_VDP
+% file: params_update_control_test.m
+% author: Federico Oliva
+% date: 22/06/2022
+% description: this function updates the estimated parameters on an
+% unstable LTI model
+% INPUT:
+% params: structure with all the necessary parameters 
+% x: state vector
+% OUTPUT:
+% params_out: updated structure with the new model parameters
+function params_out = params_update_TCV_Zaccarian(params,x)
+
+    % assign params
+    params_out = params;  
+    
+    % update psi
+
+    %%% update the denominator %%%
+    params_out.Psi = x(end-params.NumGamma-params.NumPsi+1:end-params.NumGamma)';
+
+    % update An
+    params_out.W_An = tf(params.num_An, params_out.Psi);
+    params_out.Anstar = dcgain(params_out.W_An);
+    params_out.sys_An = ss(params_out.W_An);
+    params_out.A_an = params_out.sys_An.A;
+    params_out.B_an = params_out.sys_An.B;
+    params_out.C_an = params_out.sys_An.C;
+    params_out.D_an = params_out.sys_An.D;
+    params_out.sys_An.InputName = 'v';
+    params_out.sys_An.OutputName = 'ya';    
+        
+    % update gamma
+    safeeps = 1e-10;
+    params_out.gamma = x(end+1-params.NumGamma:end);
+    params_out.Gamma = diag(params_out.gamma+safeeps);
+    params_out.A_op = params_out.Gamma*params.sys_op_def.A;
+    params_out.B_op = params_out.Gamma*params.sys_op_def.B;
+    params_out.C_op = params.sys_op_def.C;
+    params_out.D_op = params.sys_op_def.D;
+    params_out.sys_op = ss(params_out.A_op,params_out.B_op,params_out.C_op,params_out.D_op);  
+    params_out.sys_op.InputName = 'yc';
+    params_out.sys_op.OutputName = 'v';
+            
+    % update CL sys
+    i = params.traj;    
+    params_out.sys_SumAll = sumblk('u = yc + ya',params.m);  
+    params_out.sys_pert(i).sys_CL_Allu = connect(params.sys_Sum,params.sys_C_err,params_out.sys_op,params_out.sys_An,params.sys_SumAll,params.sys_pert(i).sys_P,'r','u');
+    params_out.sys_pert(i).sys_CL_All = connect(params.sys_Sum,params.sys_C_err,params_out.sys_op,params_out.sys_An,params.sys_SumAll,params.sys_pert(i).sys_P,'r','y');  
+
+
+end
\ No newline at end of file
diff --git a/Lib/models/TCV/ZaccarianInput/params_update_TCV_Zaccarian_Nonlinear.m b/Lib/models/TCV/ZaccarianInput/params_update_TCV_Zaccarian_Nonlinear.m
new file mode 100644
index 0000000..8a9416d
--- /dev/null
+++ b/Lib/models/TCV/ZaccarianInput/params_update_TCV_Zaccarian_Nonlinear.m
@@ -0,0 +1,36 @@
+%% PARAMS_UPDATE_OSCILLATOR_VDP
+% file: params_update_control_test.m
+% author: Federico Oliva
+% date: 22/06/2022
+% description: this function updates the estimated parameters on an
+% unstable LTI model
+% INPUT:
+% params: structure with all the necessary parameters 
+% x: state vector
+% OUTPUT:
+% params_out: updated structure with the new model parameters
+function params_out = params_update_TCV_Zaccarian_Nonlinear(params,x)
+
+    % assign params
+    params_out = params;  
+    
+    % update psi
+
+    %%% update the denominator %%%
+    params_out.Psi = x(end-params.NumGamma-params.NumPsi+1:end-params.NumGamma)';
+
+    % update An
+    params_out.W_An = tf(params.num_An, params_out.Psi);
+    params_out.Anstar = dcgain(params_out.W_An);
+    params_out.sys_An = ss(params_out.W_An);
+    params_out.A_an = params_out.sys_An.A;
+    params_out.B_an = params_out.sys_An.B;
+    params_out.C_an = params_out.sys_An.C;
+    params_out.D_an = params_out.sys_An.D; 
+        
+    % update gamma
+    safeeps = 1e-10;
+    params_out.gamma = x(end+1-params.NumGamma:end);
+    params_out.Gamma = diag(params_out.gamma+safeeps);
+
+end
\ No newline at end of file
diff --git a/Lib/models/TCV/ZaccarianOutput/model_TCV_Zaccarian_outAll.m b/Lib/models/TCV/ZaccarianOutput/model_TCV_Zaccarian_outAll.m
new file mode 100644
index 0000000..aa49481
--- /dev/null
+++ b/Lib/models/TCV/ZaccarianOutput/model_TCV_Zaccarian_outAll.m
@@ -0,0 +1,72 @@
+%% MODEL_OSCILLATOR_VDP
+% file: model_control_test.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function describes the dynamics equation of an unstable
+% LTI model
+% INPUT:
+% t: time instant
+% x: state vector
+% params: structure with all the necessary parameters 
+% obs: observer class instance (may be not used)
+% OUTPUT:
+% x_dot: dynamics equations
+function x_dot = model_TCV_Zaccarian_outAll(t,x,params,obs)
+
+    % init the dynamics 
+    x_dot = zeros(length(x),1);
+    
+    % compute the time index
+    tdiff = obs.setup.time-t;   
+    pos = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+    pos = max(1,min(pos,size(obs.init.Y_full_story(obs.init.traj).val,3)));
+    
+    % compute y out of p
+    y = obs.setup.measure(x,params,t,obs.init.input_story(obs.init.traj).val(:,max(1,pos-1)));
+    
+    % compute the reference (Sigma_r)
+    range = 1:params.dim_state_r;
+    x_dot(range,:) = params.Ar*x(range,:) + params.Br*0; 
+    r = params.Cr*x(range,:) + params.Dr*0;    
+    
+    % compute the control (Sigma_c)
+    range = params.dim_state_r+1:params.dim_state_r+params.dim_state_c;
+    rbar = r(params.q_pos);
+    ybar = y(params.q_pos);    
+    e = ybar;
+    obs.init.error_story_ref(obs.init.traj).val(:,pos) = e;
+    x_dot(range,:) = params.Ac*x(range,:) + params.Bc*e + params.Bcr*rbar; 
+    u = params.Cc*x(range,:) + params.Dc*e + params.Dcr*rbar;           
+    obs.init.input_default_story(obs.init.traj).val(:,pos) = u;  
+    
+    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+    %%%%%%%%%%%% ALLOCATION %%%%%%%%%%%
+    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+    
+    %%%%%% compute the optimizer %%%%%%
+    range = params.dim_state_r+params.dim_state_c+1:params.dim_state_r+params.dim_state_c+params.dim_state_op;         
+    % gradient    
+    % static grad
+    ytilde = y(params.eta_pos);
+    grad = transpose(params.Pperp_bar)*transpose(params.Pstar_tilde)*params.R*params.Pstar_tilde;
+    x_dot(range,:) = params.gamma*(grad*params.Pperp_bar*x(range,:) + grad*u);      
+    v = x(range,:);
+    obs.init.optimizer_story(obs.init.traj).val(:,pos) = v;
+    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
+    
+    %%%%%% compute the annihilator %%%%%%
+    range = params.dim_state_r+params.dim_state_c+params.dim_state_op+1:params.dim_state_r+params.dim_state_c+params.dim_state_op+params.dim_state_an;
+    x_dot(range,:) = params.A_an*x(range,:) + params.B_an*v;    
+    ya = params.C_an*x(range,:) + params.D_an*v;
+    obs.init.allocator_story(obs.init.traj).val(:,pos) = ya;
+        
+    % sum input
+%     u_new = u;
+    u_new = u+ya;
+    obs.init.input_story(obs.init.traj).val(:,pos) = u_new;  
+    
+    % model dynamics  (Sigma p)
+    range = params.dim_state_r+params.dim_state_c+params.dim_state_op+params.dim_state_an+1:params.dim_state_r+params.dim_state_c+params.dim_state_op+params.dim_state_an+params.n;
+    x_dot(range,:) = params.sys_pert(obs.init.traj).A*x(range,:) + params.sys_pert(obs.init.traj).B*(u_new);          
+       
+end
\ No newline at end of file
diff --git a/Lib/models/TCV/ZaccarianOutput/model_TCV_reference_Zaccarian_outAll.m b/Lib/models/TCV/ZaccarianOutput/model_TCV_reference_Zaccarian_outAll.m
new file mode 100644
index 0000000..7a9facd
--- /dev/null
+++ b/Lib/models/TCV/ZaccarianOutput/model_TCV_reference_Zaccarian_outAll.m
@@ -0,0 +1,67 @@
+%% MODEL_OSCILLATOR_VDP
+% file: model_control_test.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function describes the dynamics equation of an unstable
+% LTI model
+% INPUT:
+% t: time instant
+% x: state vector
+% params: structure with all the necessary parameters 
+% obs: observer class instance (may be not used)
+% OUTPUT:
+% x_dot: dynamics equations
+function x_dot = model_TCV_reference_Zaccarian_outAll(tspan,x,params,obs)
+
+    % init the dynamics         
+    x_dot = repmat(x,1,max(1,length(tspan)-1));     
+    t = tspan(1);
+    
+    % compute the time index
+    for i=1:length(tspan)
+        tdiff = obs.setup.time-tspan(i);   
+        pos(i) = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+        pos(i) = max(1,pos(i));        
+    end
+    drive_out = [];
+    params.u = params.input(tspan,drive_out,params);                               
+    
+    % shift tspan
+    tspan = tspan-tspan(1);
+    
+    % compute y out of p
+    y = obs.setup.measure_reference(x,params,t,obs.init.input_story_ref(obs.init.traj).val(:,max(1,pos-1)));
+    
+    % compute the reference (Sigma_r)
+    range = 1:params.dim_state_r;
+    x_dot(range,:) = params.Ar*x(range,:) + params.Br*0; 
+    r = params.Cr*x(range,:) + params.Dr*params.u;
+    obs.init.reference_story(obs.init.traj).val(:,pos) = r';
+    
+    % compute the control (Sigma_c)
+    range = params.dim_state_r+1:params.dim_state_r+params.dim_state_c;
+    rbar = r(params.q_pos);
+    ybar = y(params.q_pos);    
+    e = rbar-ybar;    
+    obs.init.error_story_ref(obs.init.traj).val(:,pos) = e;
+    x_dot(range,:) = params.Ac*x(range,:) + params.Bc*0 + params.Bcr*e; 
+    u = params.Cc*x(range,:) + params.Dc*0 + params.Dcr*e;            
+    obs.init.input_story_ref(obs.init.traj).val(:,pos) = u;        
+    
+    % model dynamics  (Sigma p)
+    range = params.dim_state_r+params.dim_state_c+params.dim_state_op+params.dim_state_an+1:params.dim_state_r+params.dim_state_c+params.dim_state_op+params.dim_state_an+params.n;
+    x_dot(range,:) = params.sys_pert(obs.init.traj).A*x(range,:) + params.sys_pert(obs.init.traj).B*(u);  
+    
+    obs.init.input_default_story(obs.init.traj).val(:,pos) = u';
+    obs.init.input_story_ref(obs.init.traj).val(:,pos) = u'; 
+    
+    % save error
+    rbar = r(:,params.q_pos);
+    ybar = y(:,params.q_pos);    
+    e = rbar-ybar;
+    obs.init.error_story_ref(obs.init.traj).val(:,pos) = e';
+
+%     u_prev = obs.init.input_story_ref(obs.init.traj).val(:,max(1,pos-1));
+%     obs.init.Jstory_def(obs.init.traj).val(pos) = Jcost(x,u_prev,params); 
+ 
+end
\ No newline at end of file
diff --git a/Lib/models/TCV/ZaccarianOutput/model_TCV_reference_Zaccarian_outAll_Lsim.m b/Lib/models/TCV/ZaccarianOutput/model_TCV_reference_Zaccarian_outAll_Lsim.m
new file mode 100644
index 0000000..dba2c12
--- /dev/null
+++ b/Lib/models/TCV/ZaccarianOutput/model_TCV_reference_Zaccarian_outAll_Lsim.m
@@ -0,0 +1,58 @@
+%% MODEL_OSCILLATOR_VDP
+% file: model_control_test.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function describes the dynamics equation of an unstable
+% LTI model
+% INPUT:
+% t: time instant
+% x: state vector
+% params: structure with all the necessary parameters 
+% obs: observer class instance (may be not used)
+% OUTPUT:
+% x_dot: dynamics equations
+function x_dot = model_TCV_reference_Zaccarian_outAll_Lsim(tspan,x,params,obs)
+
+    % init the dynamics         
+    x_dot = repmat(x,1,length(tspan)-1);     
+    t = tspan(1);
+    
+    % compute the time index
+    for i=1:length(tspan)
+        tdiff = obs.setup.time-tspan(i);   
+        pos(i) = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+        pos(i) = max(1,pos(i));        
+    end
+    drive_out = [];
+    params.u = params.input(tspan,drive_out,params);               
+    
+    % compute the reference (Sigma_r)
+    range = 1:params.dim_state_r;
+    x_dot(range,:) = params.Ar*x(range,:) + params.Br*0; 
+    r = params.Cr*x(range,:) + params.Dr*params.u;
+    obs.init.reference_story(obs.init.traj).val(:,pos) = r';        
+    
+    % shift tspan
+    tspan = tspan-tspan(1);
+    
+    % compute the CL plant
+    range_c = params.dim_state_r+1:params.dim_state_r + params.dim_state_c;
+    range_p = params.dim_state_r + params.dim_state_c + params.dim_state_op + params.dim_state_an + 1:params.dim_state - params.NumPsi - params.NumGamma;
+    range = [range_c, range_p];    
+    r_lsim = r;
+    x0 = x(range);
+    [y,~,xout] = lsim(params.sys_pert(obs.init.traj).sys_CL,r_lsim,tspan,x0,'foh');  
+    x_dot(range,:) = xout(2:end,:)';
+    
+    % save input story
+    [u,~,~] = lsim(params.sys_pert(obs.init.traj).sys_CLu,r_lsim,tspan,x0,'foh');  
+    obs.init.input_default_story(obs.init.traj).val(:,pos) = u';
+    obs.init.input_story_ref(obs.init.traj).val(:,pos) = u'; 
+    
+    % save error
+    rbar = r(:,params.q_pos);
+    ybar = y(:,params.q_pos);    
+    e = rbar-ybar;
+    obs.init.error_story_ref(obs.init.traj).val(:,pos) = e';
+ 
+end
\ No newline at end of file
diff --git a/Lib/models/TCV/ZaccarianOutput/params_TCV_Zaccarian_outAll.m b/Lib/models/TCV/ZaccarianOutput/params_TCV_Zaccarian_outAll.m
new file mode 100644
index 0000000..09ca104
--- /dev/null
+++ b/Lib/models/TCV/ZaccarianOutput/params_TCV_Zaccarian_outAll.m
@@ -0,0 +1,171 @@
+    %% PARAMS_OSCILLATOR_VDP
+% file: params_control_test.m
+% author: Federico Oliva
+% date: 22/06/2022
+% description: this function initialises the parameters for an unstable LTI
+% model
+% INPUT: none
+% OUTPUT:
+% params: structure with all the necessary parameters
+function params = params_TCV_Zaccarian_outAll(varargin)        
+    
+    % Zaccarian
+    % generate system
+    params.A = [-0.157 -0.094; ...
+                -0.416 -0.45];
+    params.B = [0.87 0.253 0.743; ...
+                0.39 0.354 0.65];
+    params.C = [0 1; 1 0];
+    params.D = [0 0 0; 0 0 0];
+    params.sys_P = ss(params.A,params.B,params.C,params.D);     
+
+    % generate dimensions
+    params.n = size(params.A,1);
+    params.m = size(params.B,2);
+    params.p = size(params.C,1);
+
+    % generate subsystem bar
+    params.q_pos = 1;    
+    params.q = length(params.q_pos); 
+    params.Cbar = params.C(params.q_pos,:);
+    params.Dbar = params.D(params.q_pos,:);    
+    params.sys_Pbar = ss(params.A,params.B,params.Cbar,params.Dbar);        
+    
+    % generate subsystem tilde        
+    params.eta_pos = 1:params.p;
+    params.eta_pos(params.q_pos) = [];
+    params.eta = params.p-params.q;
+    params.Ctilde = params.C(params.eta_pos,:);        
+    params.Dtilde = params.D(params.eta_pos,:);
+    params.sys_Ptilde = ss(params.A,params.B,params.Ctilde,params.Dtilde);    
+    
+    % reference signal system (Sigma_r)
+    params.dim_state_r = size(params.C,1);
+    params.Ar = zeros(params.dim_state_r);
+    params.Br = 0;    
+    params.Cr = eye(params.dim_state_r);    
+    params.Dr = 0;    
+                    
+    % input stuff
+    params.dim_input = params.m;    
+    
+    % controller system (Sigma_c)    
+    params.dim_state_c = 4;
+    params.Ac = [-1.57 0.5767 0.822 -0.65;...
+                 -0.9 -0.501 -0.94 0.802; ...
+                 0 1 -1.61 1.614; ...
+                 0 0 0 0];
+    params.Bcr = [0; 0; 0; 1];
+    params.Bc = -params.Bcr;
+    params.Cc = [1.81 -1.2 -0.46 0; ...
+                 -0.62 1.47 0.89 0; ...
+                 0 0 0 0];
+    params.Dcr = [0; 0; 0];
+    params.Dc = -params.Dcr;
+    params.sys_C = ss(params.Ac,[params.Bc params.Bcr],params.Cc,[params.Dc params.Dcr]);     
+    
+    % choose allocator
+    params.staticAll = varargin{1}{3};
+    
+    % dynamic annihilator    
+    [params.sys_An, params.W_An, params.N, params.N_An, params.num_An] = annihilator(params.sys_Pbar);                
+    % get denominator and set numbers of parameters
+    [~, D] = tfdata(params.W_An(1,1));
+    params.NumPsi = length(D{1});    
+    % params to be estimated
+    for i=1:params.NumPsi
+       tmpstr =  ['params.psi_', num2str(i) ' = D{1}(end-i+1);'];
+       eval(tmpstr);       
+    end
+    params.Psi = reshape(D{1},params.NumPsi,1);
+    % custom
+    params.Psi = [2.7225e-01 1.4015e+01 1.0018e+03 4.1827e+04]';
+    
+    % output allocation         
+    params.Pstar = dcgain(params.sys_P);
+    params.Pstar_tilde = dcgain(params.sys_Ptilde);
+    params.Pstar_bar = dcgain(params.sys_Pbar);
+    
+    if params.staticAll
+        params.Pperp_bar = null(params.Pstar_bar);
+    else
+        params.Pperp_bar = dcgain(params.sys_An);
+    end
+    
+    % optimizer (Sigma_op)    
+    params.dim_state_op = size(params.Pperp_bar,2);          
+    
+    % annihilator (Sigma_an)
+    if params.staticAll
+        params.dim_state_an = 1;
+        params.A_an = zeros(params.dim_state_an);
+        params.B_an = zeros(params.dim_state_an,params.dim_state_op);
+        params.C_an = zeros(size(params.Pperp_bar,1),params.dim_state_an);                          
+        params.D_an = params.Pperp_bar;         
+        params.sys_An = ss(params.A_an,params.B_an,params.C_an,params.D_an);
+        params.Rcoef = 1e0;
+        params.gamma = -5e-2;
+    else
+        params.A_an = params.sys_An.A;
+        params.B_an = params.sys_An.B;
+        params.C_an = params.sys_An.C;
+        params.D_an = params.sys_An.D;        
+        params.dim_state_an = size(params.A_an,1);
+        params.Rcoef = 1e10;
+        params.gamma = -1e-2;
+    end           
+    params.R = diag(params.Rcoef)*eye(params.eta); 
+                
+    % state dimension
+    params.dim_state = params.n + params.dim_state_c + params.dim_state_r + params.dim_state_op + params.dim_state_an + params.NumPsi; % + length(params.gamma) + length(params.Rcoef);
+    
+    % initial condition
+    % [x, xc, xop, xan, psi]
+    params.X(1).val(:,1) = [ones(params.dim_state_r,1); zeros(params.dim_state_c,1); zeros(params.dim_state_op,1); zeros(params.dim_state_an,1); 0*ones(params.n,1); params.Psi];
+        %params.gamma; params.Rcoef; params.Psi];
+    
+    % position in the state vector of the estimated parameters
+    params.estimated_params = [params.n + params.dim_state_c + params.dim_state_r + params.dim_state_op + params.dim_state_an + 1:params.dim_state];
+    
+    % which vars am I optimising
+    params.opt_vars = [params.n + params.dim_state_c + params.dim_state_r + params.dim_state_op + params.dim_state_an + 1:params.dim_state];    
+    
+    % set the not optimised vars
+    tmp = 1:length(params.X(1).val(:,1));
+    tmp_idx = tmp;
+    for i=1:length(params.opt_vars)
+        tmp_idx = intersect(tmp_idx,find(tmp~=params.opt_vars(i)));
+    end
+    params.nonopt_vars = tmp_idx;
+    
+    % out vars
+    params.OutDim = 2;
+    params.OutDim_compare = [params.q_pos];    
+    
+    % plot vars (used to plot the state estimation. When the parameters are
+    % too many, consider to use only the true state components)
+    params.plot_vars = 1:(params.n + params.dim_state_c + params.dim_state_r + params.dim_state_op + params.dim_state_an);
+    params.plot_params = (params.n + params.dim_state_c + params.dim_state_r + params.dim_state_op + params.dim_state_an + 1):params.dim_state;   
+    
+    % number of reference trajectories (under development)
+    params.Ntraj = 10;
+    
+    % perturbed models
+    params.sys_pert(1).A = params.A;
+    params.sys_pert(1).B = params.B;
+    params.sys_pert(1).C = params.C;
+    params.sys_pert(1).D = params.D;
+    params.sys_pert(1).sys = params.sys_P;
+    
+    % pert perc
+    params.pert_perc = 0.05;
+    for i=2:params.Ntraj
+        params.sys_pert(i).A = params.A.*(1+params.pert_perc*randn(size(params.A)));
+        params.sys_pert(i).B = params.B.*(1+params.pert_perc*randn(size(params.B)));
+        params.sys_pert(i).C = params.C;
+        params.sys_pert(i).D = params.D;
+        params.sys_pert(i).sys = ss(params.sys_pert(i).A,params.sys_pert(i).B,params.sys_pert(i).C,params.sys_pert(i).D);        
+        
+        params.X(i).val(:,1) = params.X(1).val(:,1);
+    end
+end
\ No newline at end of file
diff --git a/Lib/models/TCV/ZaccarianOutput/params_update_TCV_Zaccarian_outAll.m b/Lib/models/TCV/ZaccarianOutput/params_update_TCV_Zaccarian_outAll.m
new file mode 100644
index 0000000..985df53
--- /dev/null
+++ b/Lib/models/TCV/ZaccarianOutput/params_update_TCV_Zaccarian_outAll.m
@@ -0,0 +1,45 @@
+%% PARAMS_UPDATE_OSCILLATOR_VDP
+% file: params_update_control_test.m
+% author: Federico Oliva
+% date: 22/06/2022
+% description: this function updates the estimated parameters on an
+% unstable LTI model
+% INPUT:
+% params: structure with all the necessary parameters 
+% x: state vector
+% OUTPUT:
+% params_out: updated structure with the new model parameters
+function params_out = params_update_TCV_Zaccarian_outAll(params,x)
+
+    % assign params
+    params_out = params; 
+    
+    if ~params.staticAll
+        
+%         params_out.Rcoef = x(end+1-params.NumPsi-length(params.Rcoef));
+%         params_out.R = diag(params_out.Rcoef);
+%         params_out.gamma = x(end+1-params.NumPsi-length(params.gamma)-length(params.Rcoef));
+
+        % update psi
+        for i=1:params.NumPsi
+            tmpstr =  ['params_out.psi_', num2str(i) ' = x(end-', num2str(params.NumPsi-i), ');'];
+            eval(tmpstr); 
+        end
+
+        %%% update the denominator %%%
+        Psi = 'den = [';
+        for i=1:params.NumPsi
+            Psi = [Psi, 'params_out.psi_', num2str(i), ','];
+        end
+        Psi = [Psi(1:end-1), '];'];
+        eval(Psi);    
+
+        % update An
+        W_An = tf(params.num_An, den);
+        params_out.sys_An = ss(W_An);
+        params_out.A_an = params_out.sys_An.A;
+        params_out.B_an = params_out.sys_An.B;
+        params_out.C_an = params_out.sys_An.C;
+        params_out.D_an = params_out.sys_An.D;
+    end
+end
\ No newline at end of file
diff --git a/Lib/models/VDP/model_oscillator_VDP.m b/Lib/models/VDP/model_oscillator_VDP.m
index 3ccd260..72398d6 100644
--- a/Lib/models/VDP/model_oscillator_VDP.m
+++ b/Lib/models/VDP/model_oscillator_VDP.m
@@ -11,18 +11,27 @@
 % obs: observer class instance (may be not used)
 % OUTPUT:
 % x_dot: dynamics equations
-function x_dot = model_oscillator_VDP(t,x,params,obs)
+function x_dot = model_oscillator_VDP(tspan,x,params,obs)
 
     % init the dynamics 
     x_dot = zeros(length(x),1);
+
+    % compute the time index
+    for i=1:length(tspan)
+        tdiff = obs.setup.time-tspan(i);   
+        pos(i) = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+        pos(i) = max(1,pos(i));        
+    end    
     
     % compute the control
-    params.u = params.input(t,x,params);
+    params.u = params.input(tspan,x,params);
+    obs.init.input_story(obs.init.traj).val(:,pos) = params.u;
+    obs.init.input_story_ref(obs.init.traj).val(:,pos) = params.u;
     
     % model dynamics
     x_dot(1) = params.eps*(x(2) + params.u(1));
-    x_dot(2) = params.eps*(-x(1) + params.mu*(1-x(1)^2)*x(2) + params.u(2));
+    x_dot(2) = params.eps*(-x(1) + x(3)*(1-x(1)^2)*x(2) + params.u(2));
     
     % params dynamics (under development)
-    % x_dot(3) = 0.1 + 0.05*randn(1);    % random walk
+    x_dot(3) = params.A_mu*cos(params.F_mu*tspan(1)+params.Phi_mu);    % oscillates
 end
\ No newline at end of file
diff --git a/Lib/models/VDP/params_oscillator_VDP.m b/Lib/models/VDP/params_oscillator_VDP.m
index dda93b1..65ed2a6 100644
--- a/Lib/models/VDP/params_oscillator_VDP.m
+++ b/Lib/models/VDP/params_oscillator_VDP.m
@@ -10,8 +10,11 @@
 function params = params_oscillator_VDP
 
     % system parameters
-    params.mu = 0.5;
-    params.eps = 1;
+    params.mu = 0;
+    params.A_mu = 2;
+    params.F_mu = 0.5;
+    params.Phi_mu = 0;
+    params.eps = 5;
     
     % control parameters
     params.K1 = 0.1;
@@ -21,10 +24,18 @@
     params.Ntraj = 1;
     
     % state dimension
-    params.dim_state = 3;
+    params.dim_state = 5;
+
+    % input dim
+    params.dim_input = 2;
+
+    % output dim
+    params.OutDim = 1;
+    params.OutDim_compare = [1];
+    params.observed_state = 2;    
     
     % initial condition
-    params.X(1).val(:,1) = [1;1;params.mu];
+    params.X(1).val(:,1) = [1;1;params.mu;params.A_mu;params.F_mu];
     
     % same initial condition for all the trajectories (under development)
     for traj=2:params.Ntraj
@@ -32,10 +43,10 @@
     end
     
     % position in the state vector of the estimated parameters
-    params.estimated_params = [3];
+    params.estimated_params = [4:5];
     
     % which vars am I optimising
-    params.opt_vars = [1:3];
+    params.opt_vars = [1:3,4:5];
     
     % set the not optimised vars
     tmp = 1:length(params.X(1).val(:,1));
@@ -47,5 +58,7 @@
     
     % plot vars (used to plot the state estimation. When the parameters are
     % too many, consider to use only the true state components)
-    params.plot_vars = params.dim_state;
-end
\ No newline at end of file
+    params.plot_vars = 1:3;
+    params.plot_params = 4:5;
+    params.multi_traj_var = params.nonopt_vars;
+end
diff --git a/Lib/models/VDP/params_update_oscillator_VDP.m b/Lib/models/VDP/params_update_oscillator_VDP.m
index 6211890..604a930 100644
--- a/Lib/models/VDP/params_update_oscillator_VDP.m
+++ b/Lib/models/VDP/params_update_oscillator_VDP.m
@@ -15,5 +15,7 @@
     params_out = params;
     
     % update the nonlinearity
-    params_out.mu = x(3);
+    params_out.A_mu = x(4);
+    params_out.F_mu = x(5);
+%     params_out.Phi_mu = x(6);    
 end
\ No newline at end of file
diff --git a/Lib/models/VolterraLotka/model_VolterraLotka.m b/Lib/models/VolterraLotka/model_VolterraLotka.m
new file mode 100644
index 0000000..1a2d85e
--- /dev/null
+++ b/Lib/models/VolterraLotka/model_VolterraLotka.m
@@ -0,0 +1,34 @@
+%% MODEL_OSCILLATOR_VDP
+% file: model_oscillator_VDP.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function describes the dynamics equation of a Van der 
+% Pol oscillator
+% INPUT:
+% t: time instant
+% x: state vector
+% params: structure with all the necessary parameters 
+% obs: observer class instance (may be not used)
+% OUTPUT:
+% x_dot: dynamics equations
+function [x_dot, x] = model_VolterraLotka(tspan,x,params,obs)
+
+    % init the dynamics 
+    x_dot = zeros(length(x),1);
+
+    % compute the time index
+    for i=1:length(tspan)
+        tdiff = obs.setup.time-tspan(i);   
+        pos(i) = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+        pos(i) = max(1,pos(i));        
+    end    
+    
+    % compute the control
+    params.u = params.input(tspan,x,params);
+    obs.init.input_story(obs.init.traj).val(:,pos) = params.u;
+    obs.init.input_story_ref(obs.init.traj).val(:,pos) = params.u;
+    
+    % model dynamics
+    x_dot(1) = params.a*x(1) -params.b*x(1)*x(2) + params.u(1);
+    x_dot(2) = params.c*x(1)*x(2) -params.d*x(2) + params.u(2);        
+end
\ No newline at end of file
diff --git a/Lib/models/VolterraLotka/params_VolterraLotka.m b/Lib/models/VolterraLotka/params_VolterraLotka.m
new file mode 100644
index 0000000..0275a0f
--- /dev/null
+++ b/Lib/models/VolterraLotka/params_VolterraLotka.m
@@ -0,0 +1,85 @@
+%% PARAMS_OSCILLATOR_VDP
+% file: params_oscillator_VDP.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function initialises the parameters for a Van der Pol
+% oscillator (both for simulation and observation)
+% INPUT: none
+% OUTPUT:
+% params: structure with all the necessary parameters
+function params = params_VolterraLotka
+
+    % system parameters
+    params.mu = 0;
+    params.a = 1*0.01;
+    params.b = 1*0.005;
+    params.c = 1*0.001;
+    params.d = 1*0.05;
+
+    % prova a non fa morire tutti, come fai?
+
+    % final
+    params.mu = 0;
+    params.a = 1*0.3;
+    params.b = 1*0.005;
+    params.c = 1*0.004;
+    params.d = 1*0.1;
+
+    G0 = 100;
+    L0 = 100;
+
+    %     G0 = 0;
+    %     L0 = 0;
+
+%     G0 = params.d/params.c;
+%     L0 = params.a/params.b;
+
+    % control
+    params.K1 = 1*-1;
+    params.K2 = 0*1;
+    params.umax = 50;
+    params.target = [50, 50];
+        
+    
+    % number of reference trajectories (under development)
+    params.Ntraj = 1;
+    
+    % state dimension
+    params.dim_state = 2;
+
+    % input dim
+    params.dim_input = 2;
+
+    % output dim
+    params.OutDim = 2;
+    params.OutDim_compare = [1 2];
+    params.observed_state = [1 2];    
+    
+    % initial condition
+    params.X(1).val(:,1) = [G0;L0];
+    
+    % same initial condition for all the trajectories (under development)
+    for traj=2:params.Ntraj
+        params.X(traj).val(:,1) = params.X(traj-1).val(:,1);
+    end
+    
+    % position in the state vector of the estimated parameters
+    params.estimated_params = [];
+    
+    % which vars am I optimising
+    params.opt_vars = [];
+    
+    % set the not optimised vars
+    tmp = 1:length(params.X(1).val(:,1));
+    tmp_idx = tmp;
+    for i=1:length(params.opt_vars)
+        tmp_idx = intersect(tmp_idx,find(tmp~=params.opt_vars(i)));
+    end
+    params.nonopt_vars = tmp_idx;
+    
+    % plot vars (used to plot the state estimation. When the parameters are
+    % too many, consider to use only the true state components)
+    params.plot_vars = 1:3;
+    params.plot_params = 4:5;
+    params.multi_traj_var = params.nonopt_vars;
+end
diff --git a/Lib/models/VolterraLotka/params_update_VolterraLotka.m b/Lib/models/VolterraLotka/params_update_VolterraLotka.m
new file mode 100644
index 0000000..af37345
--- /dev/null
+++ b/Lib/models/VolterraLotka/params_update_VolterraLotka.m
@@ -0,0 +1,21 @@
+%% PARAMS_UPDATE_OSCILLATOR_VDP
+% file: params_update_oscillator_VDP.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function updates the estimated parameters on a Van der 
+% Pol oscillator
+% INPUT:
+% params: structure with all the necessary parameters 
+% x: state vector
+% OUTPUT:
+% params_out: updated structure with the new model parameters
+function params_out = params_update_VolterraLotka(params,x)
+
+    % assign params
+    params_out = params;
+    
+    % update the nonlinearity
+%     params_out.A_mu = x(4);
+%     params_out.F_mu = x(5);
+%     params_out.Phi_mu = x(6);    
+end
\ No newline at end of file
diff --git a/Lib/models/VolterraLotka/plot_VolterraLotka.m b/Lib/models/VolterraLotka/plot_VolterraLotka.m
new file mode 100644
index 0000000..c1f3838
--- /dev/null
+++ b/Lib/models/VolterraLotka/plot_VolterraLotka.m
@@ -0,0 +1,38 @@
+%%
+function plot_VolterraLotka(obs,control)
+
+    set(0,'DefaultFigureWindowStyle','docked');   
+
+    % states
+    figure()
+    hold on
+    grid on
+    plot(obs.setup.time,obs.init.X(1).val(1,:),'LineWidth',2)
+    plot(obs.setup.time,obs.init.X(1).val(2,:),'LineWidth',2)
+    if control
+        set(gca,'ColorOrderIndex',1)
+        plot(obs.setup.time(1:end-1),obs.init.input_story_ref.val(1,:),'--','LineWidth',2)
+        plot(obs.setup.time(1:end-1),obs.init.input_story_ref.val(2,:),'--','LineWidth',2)
+    end
+    set(gca,'fontsize', 20)  
+    if ~control
+        legend('G','L')
+    else
+        legend('G','L','u_G','u_L')
+    end
+    xlabel('time')
+    ylabel('animals')
+
+    % phase
+    figure()
+    hold on
+    grid on
+    plot(obs.init.X(1).val(1,1),obs.init.X(1).val(2,1),'ro','LineWidth',2,'MarkerSize',5);   
+    plot(obs.init.X(1).val(1,:),obs.init.X(1).val(2,:),'b','LineWidth',2)   
+    plot(obs.init.X(1).val(1,end),obs.init.X(1).val(2,end),'ko','LineWidth',2,'MarkerSize',5);   
+    set(gca,'fontsize', 20)  
+    xlabel('G')
+    ylabel('L')
+
+
+end
\ No newline at end of file
diff --git a/Lib/models/battery/ECM_2021a.slx b/Lib/models/battery/ECM_2021a.slx
new file mode 100644
index 0000000..944bd0d
Binary files /dev/null and b/Lib/models/battery/ECM_2021a.slx differ
diff --git a/Lib/models/battery/ECM_2023a.slx b/Lib/models/battery/ECM_2023a.slx
new file mode 100644
index 0000000..5bb6caa
Binary files /dev/null and b/Lib/models/battery/ECM_2023a.slx differ
diff --git a/Lib/models/battery/data_loader.m b/Lib/models/battery/data_loader.m
new file mode 100644
index 0000000..06b2add
--- /dev/null
+++ b/Lib/models/battery/data_loader.m
@@ -0,0 +1,67 @@
+function [final_input_output_signal_data, simout_struct] = data_loader(input_output_signal_filepath)
+
+    % Read the specified sheet from the Excel file into a table
+    input_output_signal_data = readtable(input_output_signal_filepath, 'Sheet','Channel_1-008');
+
+    % Change the sign of the current to consider positive as discharge and negative as charge
+    input_output_signal_data.Current_A_ = -1 * input_output_signal_data.Current_A_;
+
+    % Find the starting index where Step_Index equals 7
+    startIndex = find(input_output_signal_data.Step_Index == 7, 1, 'first');
+    extracted_data = input_output_signal_data(startIndex:end, :);
+
+    % Select relevant columns for further processing
+    final_input_output_signal_data = extracted_data(:, {'Current_A_', 'Date_Time', 'Step_Time_s_', 'Voltage_V_'});
+    nRows = size(final_input_output_signal_data, 1);
+
+    % Generate a time vector in seconds
+    timeInSeconds = 0:(nRows-1);
+    final_input_output_signal_data.Step_Time_s_ = timeInSeconds';
+
+    % Define battery parameters
+    params.InputAmplitude = -1;
+    params.C_n_h_nominal = 2.0 * abs(params.InputAmplitude);  % Ampere-hour
+    params.C_n_nominal = params.C_n_h_nominal * 3600;        % Convert to Ampere-second
+
+    % Calculate SOC for each time step
+    soc = 0.8 - cumsum(final_input_output_signal_data.Current_A_ / (3600 * params.C_n_h_nominal));
+
+    % Add the SOC column to the data table
+    final_input_output_signal_data.SOC = soc;
+
+    % Create a timeseries object for SOC
+    ECM_soc = timeseries(soc, final_input_output_signal_data.Step_Time_s_, 'Name','ECM_soc');
+
+    %% **Filtering Step: Exclude Data with SOC < 0.09 Based on ECM_soc.data**
+    
+    % Identify rows where SOC is greater than or equal to 0.09
+    valid_idx = ECM_soc.Data >= 0.09;
+    
+    % Filter the data table to include only valid rows
+    final_input_output_signal_data = final_input_output_signal_data(valid_idx, :);
+    
+    % Optional: Reset the time vector after filtering (if needed)
+    % final_input_output_signal_data.Step_Time_s_ = 0:(height(final_input_output_signal_data)-1)';
+    
+    % Update the ECM_soc timeseries to reflect the filtered SOC
+    ECM_soc = timeseries(final_input_output_signal_data.SOC, final_input_output_signal_data.Step_Time_s_, 'Name','ECM_soc');
+
+    %% **Update Timeseries Objects with Filtered Data**
+    
+    % Create timeseries objects for Voltage and Current with the filtered data
+    ECM_Vb = timeseries(final_input_output_signal_data.Voltage_V_, final_input_output_signal_data.Step_Time_s_, 'Name', 'ECM_Vb');
+    u = timeseries(final_input_output_signal_data.Current_A_, final_input_output_signal_data.Step_Time_s_, 'Name', 'u');
+
+    % For now, no additional noise is added
+    ECM_Vb_noise = ECM_Vb;
+    
+    %% **Prepare the Output Structure for Simulink**
+    
+    simout = struct('ECM_Vb_noise', ECM_Vb_noise, ...
+                    'ECM_Vb', ECM_Vb, ...
+                    'u', u, ...
+                    'ECM_soc', ECM_soc);
+    
+    simout_struct = struct('simout', simout, 'tout', final_input_output_signal_data.Step_Time_s_);
+
+end
diff --git a/Lib/models/battery/first_guess.m b/Lib/models/battery/first_guess.m
new file mode 100644
index 0000000..ceda832
--- /dev/null
+++ b/Lib/models/battery/first_guess.m
@@ -0,0 +1,122 @@
+%% main script
+
+function params_out = first_guess(params,params_sim)
+
+    % set time
+    t_pos = 1;
+    traj = 1;
+    
+    % set Npoint for cloud
+    Npoint = 100;
+    
+    % define noise
+    perc = 1;
+    sigma = 0*[0.05 0.2 0.2 0.2];
+    
+    % define param
+    Nparam = 4;
+    Npoly = 5;
+    CT = 0;
+    
+    % opt vars
+    opt_vars = [7:10,11:14,15:18,19:22,23:26,27:30];
+    compare_vars = 3:6;
+    update_vars = [ 7 11 15 19 23 27;...
+                    8 12 16 20 24 28;...
+                    9 13 17 21 25 29;...
+                    10 14 18 22 26 30];
+    
+    % optimset
+    myoptioptions = optimset('MaxIter', 300, 'display','off', 'MaxFunEvals',Inf,'TolFun',0,'TolX',0); 
+    
+    % generate dataset
+    x_start = params.X(traj).val(:,1);
+    x_out = gen_meas(params_sim,x_start,Npoint,perc,sigma);
+    
+    % init est
+    old_Xest = params.X(traj).val(:,1);
+    
+    for param = 1:Nparam
+        % opt vars
+        opt_vars_curr = opt_vars(param + (0:Npoly-1)*Nparam);
+        compare_vars_curr = compare_vars(param);
+        
+        % set the not optimised vars
+        tmp = 1:length(params.X(traj).val(:,1));
+        tmp_idx = tmp;
+        for i=1:length(opt_vars_curr)
+            tmp_idx = intersect(tmp_idx,find(tmp~=opt_vars_curr(i)));
+        end
+        nonopt_vars = tmp_idx;
+        
+        % evolute for tspan                
+        x0 = params.X(traj).val(:,1);
+        x0_nonopt = x_out(nonopt_vars,:);
+        x0_opt = x0(opt_vars_curr);
+
+        if param == 1
+            tmp = polyfit(x_out(1,:),x_out(compare_vars_curr,:),Npoly);
+            old_Xest(update_vars(param,1:(Npoly+1))) = flip(tmp); 
+        else 
+            tmp = polyfit(x_out(1,:),x_out(compare_vars_curr,:),Npoly-1);
+            old_Xest(update_vars(param,1:(Npoly))) = flip(tmp);             
+        end                    
+    end        
+    
+    % obs.init
+    params.cloud_Y = x_out(3:6,:);
+    params.cloud_X = x_out(1,:);
+    
+    % update params - init
+    update_vars_row = reshape(update_vars(:,1:(Npoly+1)),1,size(update_vars(:,1:(Npoly+1)),1)*size(update_vars(:,1:(Npoly+1)),2));
+    
+    % if DT
+    params.X(traj).val(update_vars_row,1) = old_Xest(update_vars_row);
+    
+    % update est params
+    params = params_update_battery_tushar(params,params.X(traj).val(:,1));       
+
+    % save the points you used for the polyfit
+    params.params_first_guess = x_out;
+
+    params_out = params;
+        
+end
+
+%%
+function x_out = gen_meas(params,x,Npoint,perc,sigma)    
+
+          
+    
+    NSeg = length(params.input_data.SOC)-1;
+    NPointsSeg = floor(Npoint/NSeg);
+    
+    % initialize state
+    x_out = zeros(length(x),NSeg*NPointsSeg);
+    
+    % create cloud
+    for i=1:NSeg
+        for j=1:NPointsSeg
+            % generate SOC        
+            x_out(1,(i-1)*NPointsSeg+j) =  unifrnd(params.input_data.SOC(i),params.input_data.SOC(i+1));            
+            
+            % generate points (noiseless)
+            ref(3) = interp1(params.input_data.SOC(i:i+1), params.input_data.OCV(i:i+1), x_out(1,(i-1)*NPointsSeg+j));
+            ref(4) = interp1(params.input_data.SOC(i:i+1), params.input_data.R0(i:i+1), x_out(1,(i-1)*NPointsSeg+j));
+            ref(5) = interp1(params.input_data.SOC(i:i+1), params.input_data.R1(i:i+1), x_out(1,(i-1)*NPointsSeg+j));
+            ref(6) = interp1(params.input_data.SOC(i:i+1), params.input_data.C1(i:i+1), x_out(1,(i-1)*NPointsSeg+j));
+            
+            x_out(3,(i-1)*NPointsSeg+j) = ref(3)*(1+sigma(1)*randn);
+            x_out(4,(i-1)*NPointsSeg+j) = ref(4)*(1+sigma(2)*randn);
+            x_out(5,(i-1)*NPointsSeg+j) = ref(5)*(1+sigma(3)*randn);
+            x_out(6,(i-1)*NPointsSeg+j) = ref(6)*(1+sigma(4)*randn);
+        
+        end
+    end  
+
+    [x_out(1,:), I] = sort(x_out(1,:));
+    x_out(3,:) = x_out(3,I);
+    x_out(4,:) = x_out(4,I);
+    x_out(5,:) = x_out(5,I);
+    x_out(6,:) = x_out(6,I);
+end
\ No newline at end of file
diff --git a/Lib/models/battery/model_battery_calce.m b/Lib/models/battery/model_battery_calce.m
new file mode 100644
index 0000000..5233075
--- /dev/null
+++ b/Lib/models/battery/model_battery_calce.m
@@ -0,0 +1,53 @@
+%% MODEL_BATTERY
+% file: model_battery.m
+% author: Federico Oliva
+% date: 27/05/2022
+% description: this function describes the dynamics equation of a Van der 
+% Pol oscillator
+% INPUT:
+% t: time instant
+% x: state vector
+% params: structure with all the necessary parameters 
+% obs: observer class instance (may be not used)
+% OUTPUT:
+% x_dot: dynamics equations
+function x_dot = model_battery_calce(t,x,params,obs)
+
+    % init the dynamics 
+    x_dot = x;  
+    
+    % compute the control
+    params.u = 1*params.input(t,x,params,obs);        
+
+    % save input
+    tdiff = obs.setup.time-t;   
+    pos = find(abs(tdiff) == min(abs(tdiff)),1,'first'); 
+    pos = min(pos,size(obs.init.Y_full_story(obs.init.traj).val,3));
+    obs.init.input_story(obs.init.traj).val(:,pos) = params.u;
+    
+    % model dynamics - discrete
+    % Zk (SOC)
+    if (x(1,:) > 0) || 0   
+        x_dot(1,:) = x(1,:) - params.eta * obs.setup.Ts *params.u(1,:)/params.C_n;                        
+    else
+        x_dot(1,:) = 0;              
+    end    
+
+    %%% model equations %%%
+    tau_1 = x(5,:).*x(6,:);
+    a1 = exp(-obs.setup.Ts/tau_1);
+    b1 = x(5,:).*(1 - exp(-obs.setup.Ts/tau_1));
+    % V1 (voltage RC)
+    x_dot(2,:) = a1*x(2,:) + b1*params.u(1,:);
+    
+    x_dot(3,:) = params.alpha_Voc + params.beta_Voc*x(1,:) + params.gamma_Voc*(x(1,:).^2) + params.delta_Voc*(x(1,:).^3) + params.eps_Voc*(x(1,:).^4) + params.xi_Voc*(x(1,:).^5);
+    x_dot(4,:) = params.alpha_R0 + params.beta_R0*x(1,:) + params.gamma_R0*(x(1,:).^2) + params.delta_R0*(x(1,:).^3) + params.eps_R0*(x(1,:).^4) + params.xi_R0*(x(1,:).^5);
+    x_dot(5,:) = params.alpha_R1 + params.beta_R1*x(1,:) + params.gamma_R1*(x(1,:).^2) + params.delta_R1*(x(1,:).^3) + params.eps_R1*(x(1,:).^4) + params.xi_R1*(x(1,:).^5);
+    x_dot(6,:) = params.alpha_C1 + params.beta_C1*x(1,:) + params.gamma_C1*(x(1,:).^2) + params.delta_C1*(x(1,:).^3) + params.eps_C1*(x(1,:).^4) + params.xi_C1*(x(1,:).^5);
+        
+
+    
+    
+    
+
+end
\ No newline at end of file
diff --git a/Lib/models/battery/model_battery_tushar.m b/Lib/models/battery/model_battery_tushar.m
new file mode 100644
index 0000000..baa4d16
--- /dev/null
+++ b/Lib/models/battery/model_battery_tushar.m
@@ -0,0 +1,53 @@
+%% MODEL_BATTERY
+% file: model_battery.m
+% author: Federico Oliva
+% date: 27/05/2022
+% description: this function describes the dynamics equation of a Van der 
+% Pol oscillator
+% INPUT:
+% t: time instant
+% x: state vector
+% params: structure with all the necessary parameters 
+% obs: observer class instance (may be not used)
+% OUTPUT:
+% x_dot: dynamics equations
+function x_dot = model_battery_tushar(t,x,params,obs)
+
+    % init the dynamics 
+    x_dot = x;  
+    
+    % compute the control
+    params.u = 1*params.input(t,x,params,obs);        
+
+    % save input
+    tdiff = obs.setup.time-t;   
+    pos = find(abs(tdiff) == min(abs(tdiff)),1,'first'); 
+    pos = min(pos,size(obs.init.Y_full_story(obs.init.traj).val,3));
+    obs.init.input_story(obs.init.traj).val(:,pos) = params.u;
+    
+    % model dynamics - discrete
+    % Zk (SOC)
+    if (x(1,:) > 0) || 0   
+        x_dot(1,:) = x(1,:) - params.eta * obs.setup.Ts *params.u(1,:)/params.C_n;                        
+    else
+        x_dot(1,:) = 0;              
+    end    
+
+    %%% model equations %%%
+    tau_1 = x(5,:).*x(6,:);
+    a1 = exp(-obs.setup.Ts/tau_1);
+    b1 = x(5,:).*(1 - exp(-obs.setup.Ts/tau_1));
+    % V1 (voltage RC)
+    x_dot(2,:) = a1*x(2,:) + b1*params.u(1,:);
+    
+    x_dot(3,:) = params.alpha_Voc + params.beta_Voc*x(1,:) + params.gamma_Voc*(x(1,:).^2) + params.delta_Voc*(x(1,:).^3) + params.eps_Voc*(x(1,:).^4) + params.xi_Voc*(x(1,:).^5);
+    x_dot(4,:) = params.alpha_R0 + params.beta_R0*x(1,:) + params.gamma_R0*(x(1,:).^2) + params.delta_R0*(x(1,:).^3) + params.eps_R0*(x(1,:).^4) + params.xi_R0*(x(1,:).^5);
+    x_dot(5,:) = params.alpha_R1 + params.beta_R1*x(1,:) + params.gamma_R1*(x(1,:).^2) + params.delta_R1*(x(1,:).^3) + params.eps_R1*(x(1,:).^4) + params.xi_R1*(x(1,:).^5);
+    x_dot(6,:) = params.alpha_C1 + params.beta_C1*x(1,:) + params.gamma_C1*(x(1,:).^2) + params.delta_C1*(x(1,:).^3) + params.eps_C1*(x(1,:).^4) + params.xi_C1*(x(1,:).^5);
+        
+
+    
+    
+    
+
+end
\ No newline at end of file
diff --git a/Lib/models/battery/params_battery_calce.m b/Lib/models/battery/params_battery_calce.m
new file mode 100644
index 0000000..50546e3
--- /dev/null
+++ b/Lib/models/battery/params_battery_calce.m
@@ -0,0 +1,132 @@
+%% PARAMS_BATTERY
+% file: params_battery.m
+% author: Federico Oliva
+% date: 27/05/2022
+% description: this function initialises the parameters for a battery
+% model (both for simulation and observation)
+% INPUT: none
+% OUTPUT:
+% params: structure with all the necessary parameters
+function params = params_battery_calce(params_sim)
+
+    %%%%%%%%%%% LOAD DATA OF A BATTERY EXPERIMENT %%%%%%%%%%%
+        
+    % SETUP THE EXPERIMENT  - Battery Capacity (converting Ampere-hour to Ampere-second)
+    params.InputAmplitude = -1;
+    % params.C_n_h = 2.05272*abs(params.InputAmplitude);
+    params.C_n_h = 2.00*abs(params.InputAmplitude);
+    params.C_n = params.C_n_h * 3600;                 
+
+    %%% system parameters %%%
+    % battery EMF
+    params.Voc = 3.6;
+    % ohmic internal resistance
+    params.R0 = 0.0118;
+    % polarization resistance
+    params.R1 = 0.0120;
+    % polarization capacity
+    params.C1 = 3.31111e3;    
+    % Battery charging-discharging efficiency (for Li-ion=100%)
+    params.eta = 1;  
+
+    % noise characteristics
+    noise = 0;
+    params.percNoise = noise*5e-2;
+    params.NoisePwr = noise*5e-3;
+
+    % temperature
+    params.Temperature = 313.15;
+
+    % initial SOC
+    x10 = 0.9;
+    x20 = 0.05;
+    
+    params.eps = 1;
+    params.Ts = params_sim.Ts;
+    
+    %%%%%%% SETUP THE OBSERVER %%%%%
+    % out vars - observer
+    params.OutDim = 1;
+    params.OutDim_compare = [1];
+
+    % input dim - observer
+    params.dim_input = 1;                    
+       
+    % number of reference trajectories (under development)
+    params.Ntraj = 1;
+    
+    % state dimension
+    params.dim_state = 30;
+
+    % output dimension
+    params.OutDim = 1;
+    params.OutDim_compare = 1;
+
+    % dim input
+    params.dim_input = 1;
+    
+    % initial condition
+    params.X(1).val(:,1) = zeros(params.dim_state,1);
+    params.X(1).val(1:2,1) = [x10; x20];
+    
+    % same initial condition for all the trajectories (under development)
+    for traj=2:params.Ntraj
+        params.X(traj).val(:,1) = params.X(traj-1).val(:,1);
+    end
+    
+    % position in the state vector of the estimated parameters
+    params.estimated_params = [7:30];
+    
+    % which vars am I optimising
+    params.opt_vars = [1:2 8:9 12:13 16:17 20:21];
+    
+    % set the not optimised vars
+    tmp = 1:length(params.X(1).val(:,1));
+    tmp_idx = tmp;
+    for i=1:length(params.opt_vars)
+        tmp_idx = intersect(tmp_idx,find(tmp~=params.opt_vars(i)));
+    end
+    params.nonopt_vars = tmp_idx;
+    
+    % plot vars (used to plot the state estimation. When the parameters are
+    % too many, consider to use only the true state components)
+    params.plot_vars = 1:3;
+    params.plot_params = [4:6];%[7:14];
+    params.multi_traj_var = [1:2];
+
+    % add stuff
+    params.multistart = 0;
+    params.observed_state = [];
+    
+    % same initial condition for all the trajectories (under development)
+    for traj=2:params.Ntraj
+        params.X(traj).val(:,1) = params.X(traj-1).val(:,1);
+        params.X(traj).val(params.multi_traj_var,1) = params.X(traj-1).val(params.multi_traj_var,1) + abs(params.X(traj-1).val(params.multi_traj_var,1)).*randn(length(params.multi_traj_var),1);
+%         params.X(traj).val(params.multi_traj_var,1) = params.X(traj-1).val(params.multi_traj_var,1) + abs(params.X(traj-1).val(params.multi_traj_var,1)).*[1.5; 0.5; 1.5; 0.5];
+    end
+
+    %%% import data from sim %%%
+    sim = params_sim.out;
+    params.u_sim = sim.simout.u.Data';
+    params.y_sim = sim.simout.ECM_Vb_noise.Data';
+    params.y_true_sim = sim.simout.ECM_Vb.Data';
+    params.soc_sim = sim.simout.ECM_soc.Data';
+    params.time  = sim.tout';
+
+    % set the GT params (lin interp)
+    % Original SOC data indices
+    originalIndices = linspace(1, length(params_sim.input_data.SOC), length(params_sim.input_data.SOC));
+    
+    % Target indices for resampled data, aiming for 250 points
+    targetIndices = linspace(1, length(params_sim.input_data.SOC), 1000);
+    
+    % Resampled SOC using Piecewise Cubic Hermite Interpolating Polynomial (PCHIP)
+    params.params_GT_sample = interp1(originalIndices, params_sim.input_data.SOC, targetIndices, 'pchip');
+    
+    params.params_GT_OCV = interp1(params_sim.input_data.SOC,params_sim.input_data.OCV,params.params_GT_sample);
+    params.params_GT_R0 = interp1(params_sim.input_data.SOC,params_sim.input_data.R0,params.params_GT_sample);
+    params.params_GT_R1 = interp1(params_sim.input_data.SOC,params_sim.input_data.R1,params.params_GT_sample);
+    params.params_GT_C1 = interp1(params_sim.input_data.SOC,params_sim.input_data.C1,params.params_GT_sample);
+
+    params = first_guess(params,params_sim);
+end
\ No newline at end of file
diff --git a/Lib/models/battery/params_battery_calce_fast.m b/Lib/models/battery/params_battery_calce_fast.m
new file mode 100644
index 0000000..090f772
--- /dev/null
+++ b/Lib/models/battery/params_battery_calce_fast.m
@@ -0,0 +1,128 @@
+ %% PARAMS_BATTERY
+% file: params_battery.m
+% author: Federico Oliva
+% date: 27/05/2022
+% description: this function initialises the parameters for a battery
+% model (both for simulation and observation)
+% INPUT: none
+% OUTPUT:
+% params: structure with all the necessary parameters
+function params = params_battery_calce_fast(params_sim)
+
+    %%%%%%%%%%% LOAD DATA OF A BATTERY EXPERIMENT %%%%%%%%%%%
+        
+    % SETUP THE EXPERIMENT  - Battery Capacity (converting Ampere-hour to Ampere-second)
+    params.InputAmplitude = -1;
+    params.C_n_h = 2.0*abs(params.InputAmplitude);
+    params.C_n = params.C_n_h * 3600;                 
+
+    %%% system parameters %%%
+    % battery EMF
+    params.Voc = 3.6;
+    % ohmic internal resistance
+    params.R0 = 0.0118;
+    % polarization resistance
+    params.R1 = 0.0120;
+    % polarization capacity
+    params.C1 = 3.31111e3;    
+    % Battery charging-discharging efficiency (for Li-ion=100%)
+    params.eta = 1;  
+
+    % noise characteristics
+    noise = 0;
+    params.percNoise = noise*5e-2;
+    params.NoisePwr = noise*5e-3;
+
+    % temperature
+    params.Temperature = 313.15;
+
+    % initial SOC
+    x10 = 0.9;
+    x20 = 0.05;        
+    params.eps = 1;        
+    
+    %%%%%%% SETUP THE OBSERVER %%%%%
+
+    % input dim - observer
+    params.dim_input = 1;                    
+       
+    % number of reference trajectories (under development)
+    params.Ntraj = 1;
+    
+    % state dimension
+    params.dim_state = 30;
+
+    % output dimension
+    params.OutDim = 1;
+    params.OutDim_compare = 1;
+
+    % dim input
+    params.dim_input = 1;
+    
+    % initial condition
+    params.X(1).val(:,1) = zeros(params.dim_state,1);
+    params.X(1).val(1:2,1) = [x10; x20];
+    
+    % same initial condition for all the trajectories (under development)
+    for traj=2:params.Ntraj
+        params.X(traj).val(:,1) = params.X(traj-1).val(:,1);
+    end
+    
+    % position in the state vector of the estimated parameters
+    params.estimated_params = [7:30];
+    
+    % which vars am I optimising
+    % params.opt_vars = [1:2 8:10 12:14 16:18 20:22];
+    % params.opt_vars = [2 8:9 12:13 16:17 20:21];
+    params.opt_vars = [2];
+    
+    % set the not optimised vars
+    tmp = 1:length(params.X(1).val(:,1));
+    tmp_idx = tmp;
+    for i=1:length(params.opt_vars)
+        tmp_idx = intersect(tmp_idx,find(tmp~=params.opt_vars(i)));
+    end
+    params.nonopt_vars = tmp_idx;
+    
+    % plot vars (used to plot the state estimation. When the parameters are
+    % too many, consider to use only the true state components)
+    params.plot_vars = 1:3;
+    params.plot_params = [4:6];%[7:14];
+    params.multi_traj_var = [1:2];
+
+    % add stuff
+    params.multistart = 0;
+    params.observed_state = [];
+    
+    % same initial condition for all the trajectories (under development)
+    for traj=2:params.Ntraj
+        params.X(traj).val(:,1) = params.X(traj-1).val(:,1);
+        params.X(traj).val(params.multi_traj_var,1) = params.X(traj-1).val(params.multi_traj_var,1) + abs(params.X(traj-1).val(params.multi_traj_var,1)).*randn(length(params.multi_traj_var),1);
+%         params.X(traj).val(params.multi_traj_var,1) = params.X(traj-1).val(params.multi_traj_var,1) + abs(params.X(traj-1).val(params.multi_traj_var,1)).*[1.5; 0.5; 1.5; 0.5];
+    end
+
+    %%% import data from sim %%%
+    sim = params_sim.out;
+    params.u_sim = sim.simout.u.Data';
+    params.y_sim = sim.simout.ECM_Vb_noise.Data';
+    params.y_true_sim = sim.simout.ECM_Vb.Data';
+    params.soc_sim = sim.simout.ECM_soc.Data';
+
+    % set the GT params (lin interp)
+    % Original SOC data indices
+    originalIndices = linspace(1, length(params_sim.input_data.SOC), length(params_sim.input_data.SOC));
+    
+    % Target indices for resampled data, aiming for 250 points
+    targetIndices = linspace(1, length(params_sim.input_data.SOC), 1000);
+    
+    % Resampled SOC using Piecewise Cubic Hermite Interpolating Polynomial (PCHIP)
+    params.params_GT_sample = interp1(originalIndices, params_sim.input_data.SOC, targetIndices, 'pchip');
+    
+    params.params_GT_OCV = interp1(params_sim.input_data.SOC,params_sim.input_data.OCV,params.params_GT_sample);
+    params.params_GT_R0 = interp1(params_sim.input_data.SOC,params_sim.input_data.R0,params.params_GT_sample);
+    params.params_GT_R1 = interp1(params_sim.input_data.SOC,params_sim.input_data.R1,params.params_GT_sample);
+    params.params_GT_C1 = interp1(params_sim.input_data.SOC,params_sim.input_data.C1,params.params_GT_sample);
+
+    params = first_guess(params,params_sim);
+    
+end
\ No newline at end of file
diff --git a/Lib/models/battery/params_battery_calce_slow.m b/Lib/models/battery/params_battery_calce_slow.m
new file mode 100644
index 0000000..189e1c9
--- /dev/null
+++ b/Lib/models/battery/params_battery_calce_slow.m
@@ -0,0 +1,128 @@
+%% PARAMS_BATTERY
+% file: params_battery.m
+% author: Federico Oliva
+% date: 27/05/2022
+% description: this function initialises the parameters for a battery
+% model (both for simulation and observation)
+% INPUT: none
+% OUTPUT:
+% params: structure with all the necessary parameters
+function params = params_battery_calce_slow(params_sim)
+
+    %%%%%%%%%%% LOAD DATA OF A BATTERY EXPERIMENT %%%%%%%%%%%
+        
+    % SETUP THE EXPERIMENT  - Battery Capacity (converting Ampere-hour to Ampere-second)
+    params.InputAmplitude = -1;
+    params.C_n_h = 2.0*abs(params.InputAmplitude);
+    params.C_n = params.C_n_h * 3600;                 
+
+    %%% system parameters %%%
+    % battery EMF
+    params.Voc = 3.6;
+    % ohmic internal resistance
+    params.R0 = 0.0118;
+    % polarization resistance
+    params.R1 = 0.0120;
+    % polarization capacity
+    params.C1 = 3.31111e3;    
+    % Battery charging-discharging efficiency (for Li-ion=100%)
+    params.eta = 1;  
+
+    % noise characteristics
+    noise = 0;
+    params.percNoise = noise*5e-2;
+    params.NoisePwr = noise*5e-3;
+
+    % temperature
+    params.Temperature = 313.15;
+
+    % initial SOC
+    x10 = 0.9;
+    x20 = 0.05;
+    
+    params.eps = 1;        
+    
+    %%%%%%% SETUP THE OBSERVER %%%%%    
+    % input dim - observer
+    params.dim_input = 1;                    
+       
+    % number of reference trajectories (under development)
+    params.Ntraj = 1;
+    
+    % state dimension
+    params.dim_state = 30;
+
+    % output dimension
+    params.OutDim = 1;
+    params.OutDim_compare = 1;
+
+    % dim input
+    params.dim_input = 1;
+    
+    % initial condition
+    params.X(1).val(:,1) = zeros(params.dim_state,1);
+    params.X(1).val(1:2,1) = [x10; x20];
+    
+    % same initial condition for all the trajectories (under development)
+    for traj=2:params.Ntraj
+        params.X(traj).val(:,1) = params.X(traj-1).val(:,1);
+    end
+    
+    % position in the state vector of the estimated parameters
+    params.estimated_params = [7:30];
+    
+    % which vars am I optimising
+    % params.opt_vars = [1:2 8:10 12:14 16:18 20:22];    
+    % params.opt_vars = [1 10 14 18 22];
+    params.opt_vars = [1 8:9 12:13 16:17 20:21];
+    
+    % set the not optimised vars
+    tmp = 1:length(params.X(1).val(:,1));
+    tmp_idx = tmp;
+    for i=1:length(params.opt_vars)
+        tmp_idx = intersect(tmp_idx,find(tmp~=params.opt_vars(i)));
+    end
+    params.nonopt_vars = tmp_idx;
+    
+    % plot vars (used to plot the state estimation. When the parameters are
+    % too many, consider to use only the true state components)
+    params.plot_vars = 1:3;
+    params.plot_params = [4:6];%[7:14];
+    params.multi_traj_var = [1:2];
+
+    % add stuff
+    params.multistart = 0;
+    params.observed_state = [];
+    
+    % same initial condition for all the trajectories (under development)
+    for traj=2:params.Ntraj
+        params.X(traj).val(:,1) = params.X(traj-1).val(:,1);
+        params.X(traj).val(params.multi_traj_var,1) = params.X(traj-1).val(params.multi_traj_var,1) + abs(params.X(traj-1).val(params.multi_traj_var,1)).*randn(length(params.multi_traj_var),1);
+%         params.X(traj).val(params.multi_traj_var,1) = params.X(traj-1).val(params.multi_traj_var,1) + abs(params.X(traj-1).val(params.multi_traj_var,1)).*[1.5; 0.5; 1.5; 0.5];
+    end
+
+    %%% import data from sim %%%
+    sim = params_sim.out;
+    params.u_sim = sim.simout.u.Data';
+    params.y_sim = sim.simout.ECM_Vb_noise.Data';
+    params.y_true_sim = sim.simout.ECM_Vb.Data';
+    params.soc_sim = sim.simout.ECM_soc.Data';
+
+    % set the GT params (lin interp)
+    % Original SOC data indices
+    originalIndices = linspace(1, length(params_sim.input_data.SOC), length(params_sim.input_data.SOC));
+    
+    % Target indices for resampled data, aiming for 250 points
+    targetIndices = linspace(1, length(params_sim.input_data.SOC), 1000);
+    
+    % Resampled SOC using Piecewise Cubic Hermite Interpolating Polynomial (PCHIP)
+    params.params_GT_sample = interp1(originalIndices, params_sim.input_data.SOC, targetIndices, 'pchip');
+    
+    params.params_GT_OCV = interp1(params_sim.input_data.SOC,params_sim.input_data.OCV,params.params_GT_sample);
+    params.params_GT_R0 = interp1(params_sim.input_data.SOC,params_sim.input_data.R0,params.params_GT_sample);
+    params.params_GT_R1 = interp1(params_sim.input_data.SOC,params_sim.input_data.R1,params.params_GT_sample);
+    params.params_GT_C1 = interp1(params_sim.input_data.SOC,params_sim.input_data.C1,params.params_GT_sample);
+
+    params = first_guess(params,params_sim);
+    
+end
\ No newline at end of file
diff --git a/Lib/models/battery/params_battery_simulink.m b/Lib/models/battery/params_battery_simulink.m
new file mode 100644
index 0000000..55cddc9
--- /dev/null
+++ b/Lib/models/battery/params_battery_simulink.m
@@ -0,0 +1,84 @@
+%% PARAMS_BATTERY
+% file: params_battery.m
+% author: Federico Oliva
+% date: 27/05/2022
+% description: this function initialises the parameters for a battery
+% model (both for simulation and observation)
+% INPUT: none
+% OUTPUT:
+% params: structure with all the necessary parameters
+function params = params_battery_simulink
+
+    %%%%%%%%%%% LOAD DATA OF A BATTERY EXPERIMENT %%%%%%%%%%%
+    % Loading input signals and parameter data
+    input_data = load('data/ECM_parameters_updated.mat');
+    params.input_data = input_data;   
+
+    % time
+    params.Ts = 1e0;
+    params.time = 0:params.Ts:6000;
+    params.time = 0:params.Ts:2000;
+
+    % handle SOC not zero
+    params.input_data.SOC(find(params.input_data.SOC==0)) = eps;
+    
+    % GET MIN AND MAX OF OCV (MEASURE)
+    params.min_params = min([input_data.OCV;input_data.R0;input_data.R1;input_data.C1],[],2);
+    params.max_params = max([input_data.OCV;input_data.R0;input_data.R1;input_data.C1],[],2);
+
+    % SETUP THE EXPERIMENT - MODEL PERTURBARION
+    params.deltaModel = 0*0.05;
+    
+    % SETUP THE EXPERIMENT - NOMINAL DATA    
+    npoints = length(params.input_data.OCV);
+    params.input_data.OCV_nominal = params.input_data.OCV.*(1+0.1*params.deltaModel*randn(1,npoints));
+    params.input_data.R0_nominal = params.input_data.R0.*(1+params.deltaModel*randn(1,npoints));
+    params.input_data.R1_nominal = params.input_data.R1.*(1+params.deltaModel*randn(1,npoints));
+    params.input_data.C1_nominal = params.input_data.C1.*(1+params.deltaModel*randn(1,npoints));    
+           
+    % SETUP THE EXPERIMENT  - Battery Capacity (converting Ampere-hour to Ampere-second)
+    params.InputAmplitude = -1;
+    params.C_n_h_nominal = 4.1*abs(params.InputAmplitude);
+    params.C_n_nominal = params.C_n_h_nominal * 3600;         
+
+    % SETUP THE EXPERIMENT - generate modular HPPC
+    % define the input current module
+    params.input_current_Ts = 1;        
+    [HCCP, tspan, tspan_pos] = generate_HCCP(params.input_current_Ts,params.C_n_h_nominal);
+    params.startpos = tspan_pos(1);
+    params.stoppos = tspan_pos(end); 
+    params.input_current = HCCP;    
+    params.input_current_modular_period = params.stoppos-params.startpos;    
+    params.input_current_modular_time = 0:params.input_current_modular_period;
+    params.input_current_modular_time_dense = 0:params.input_current_Ts:params.input_current_modular_period;    
+    params.input_current_modular = interp1(params.input_current_modular_time,params.input_current(params.startpos:params.stoppos),params.input_current_modular_time_dense);
+
+    % slow modular HPPC - dense realization
+    params.time_slow = 1;
+    params.input_current_modular_period_slown = params.input_current_modular_period*params.time_slow;
+    params.input_current_modular_time_slown_dense = 0:params.input_current_Ts:params.input_current_modular_period_slown;
+    for i=1:length(params.input_current_modular_time_dense)
+        params.input_current_modular_slown(params.time_slow*(i-1)+1:params.time_slow*(i-1)+params.time_slow) = params.input_current_modular(i);
+    end    
+
+    % noise characteristics
+    noise = 1;
+    params.percNoise = noise*5e-2;
+    params.NoisePwr = noise*5e-3;
+
+    % temperature
+    params.Temperature = 313.15;            
+    
+    % state dimension
+    params.dim_state = 30;    
+    params.dim_out = 1;
+    params.dim_input = 1;
+    
+    % initial condition
+    params.x0_simulink = zeros(params.dim_state,1);
+
+    params.SIMstate = timeseries(zeros(params.dim_state,numel(params.time)),params.time);
+    params.SIMmeasure = timeseries(zeros(params.dim_out,numel(params.time)),params.time);
+    params.SIMinput = timeseries(zeros(params.dim_input,numel(params.time)),params.time);
+    
+end
\ No newline at end of file
diff --git a/Lib/models/battery/params_battery_simulink_calce.m b/Lib/models/battery/params_battery_simulink_calce.m
new file mode 100644
index 0000000..8a67166
--- /dev/null
+++ b/Lib/models/battery/params_battery_simulink_calce.m
@@ -0,0 +1,82 @@
+%% PARAMS_BATTERY
+% file: params_battery.m
+% author: Federico Oliva
+% date: 27/05/2022
+% description: this function initialises the parameters for a battery
+% model (both for simulation and observation)
+% INPUT: none
+% OUTPUT:
+% params: structure with all the necessary parameters
+function params = params_battery_simulink_calce
+
+    %%%%%%%%%%% LOAD DATA OF A BATTERY EXPERIMENT %%%%%%%%%%%
+
+    % loading input and output signal data
+    % '/data/CALCE/INR_18650/BJDST/SP2_25C_BJDST/11_12_2015_SP20-2_BJDST_80SOC.xlsx';
+    % '/data/CALCE/INR_18650/US06/SP2_25C_US06/11_11_2015_SP20-2_US06_80SOC.xlsx';
+    % '/data/CALCE/INR_18650/FUDS/SP2_25C_FUDS/11_06_2015_SP20-2_FUDS_80SOC.xlsx';
+    % '/data/CALCE/INR_18650/DST/SP2_25C_DST/11_05_2015_SP20-2_DST_80SOC.xlsx'
+    input_output_signal_filepath = '/data/CALCE/INR_18650/DST/SP2_25C_DST/11_05_2015_SP20-2_DST_80SOC.xlsx';
+    [final_input_output_signal_data, params.out] = data_loader(input_output_signal_filepath);
+    
+    % loading ground truth SOC-OCV data
+    soc_ocv_filepath = '/data/CALCE/INR_18650/Sample_1_SOC_incremental/SP1_25C_IC_OCV_12_2_2015.xlsx';
+    soc_ocv = readtable(soc_ocv_filepath, 'Sheet', 'SOC_OCV');
+    final_soc_ocv = soc_ocv(:, {'SOC', 'OCV'});
+
+    % Loading input signals and parameter data
+    % input_data = load('data/ECM_parameters_updated.mat');
+    input_data.OCV = transpose(final_soc_ocv.OCV);
+    input_data.SOC = transpose(final_soc_ocv.SOC);
+
+    % initial parameters
+    input_data.R0 = [0.02, 0.02166, 0.02166, 0.02166, 0.02166, 0.02166, 0.02333, 0.025, 0.02667, 0.02833, 0.03];
+    input_data.R1 =  [0.004, 0.007, 0.007, 0.007, 0.007, 0.007, 0.008, 0.009, 0.01, 0.011, 0.012];
+    input_data.C1 = [1250, 2500, 2500, 2500, 2500, 2500, 3000, 3500, 4000, 4500, 5000];
+    
+    % for consistency
+    params.input_data = input_data;
+
+    % time
+    %                                    
+    params.Ts = 1e0;
+    params.time = final_input_output_signal_data.Step_Time_s_;
+
+    % handle SOC not zero
+    params.input_data.SOC(find(params.input_data.SOC==0)) = eps;
+    
+    % GET MIN AND MAX OF OCV (MEASURE)
+    params.min_params = min([input_data.OCV;input_data.R0;input_data.R1;input_data.C1],[],2);
+    params.max_params = max([input_data.OCV;input_data.R0;input_data.R1;input_data.C1],[],2);
+
+    % SETUP THE EXPERIMENT - MODEL PERTURBARION
+    params.deltaModel = 0*0.05;
+    
+    % SETUP THE EXPERIMENT - NOMINAL DATA    
+    npoints = length(params.input_data.OCV);
+    params.input_data.OCV_nominal = params.input_data.OCV.*(1+0.1*params.deltaModel*randn(1,npoints));
+    params.input_data.R0_nominal = params.input_data.R0.*(1+params.deltaModel*randn(1,npoints));
+    params.input_data.R1_nominal = params.input_data.R1.*(1+params.deltaModel*randn(1,npoints));
+    params.input_data.C1_nominal = params.input_data.C1.*(1+params.deltaModel*randn(1,npoints));    
+             
+    % noise characteristics
+    noise = 0;
+    params.percNoise = noise*5e-2;
+    params.NoisePwr = noise*5e-3;
+
+    % temperature
+    params.Temperature = 298.15;            
+    
+    % state dimension
+    params.dim_state = 30;    
+    params.dim_out = 1;
+    params.dim_input = 1;
+    
+    % initial condition
+    params.x0_simulink = zeros(params.dim_state,1);
+
+    params.SIMstate = timeseries(zeros(params.dim_state,numel(params.time)),params.time);
+    params.SIMmeasure = timeseries(zeros(params.dim_out,numel(params.time)),params.time);
+    params.SIMinput = timeseries(zeros(params.dim_input,numel(params.time)),params.time);
+    
+end
\ No newline at end of file
diff --git a/Lib/models/battery/params_battery_tushar.m b/Lib/models/battery/params_battery_tushar.m
new file mode 100644
index 0000000..4b66c77
--- /dev/null
+++ b/Lib/models/battery/params_battery_tushar.m
@@ -0,0 +1,122 @@
+%% PARAMS_BATTERY
+% file: params_battery.m
+% author: Federico Oliva
+% date: 27/05/2022
+% description: this function initialises the parameters for a battery
+% model (both for simulation and observation)
+% INPUT: none
+% OUTPUT:
+% params: structure with all the necessary parameters
+function params = params_battery_tushar(params_sim)
+
+    %%%%%%%%%%% LOAD DATA OF A BATTERY EXPERIMENT %%%%%%%%%%%
+        
+    % SETUP THE EXPERIMENT  - Battery Capacity (converting Ampere-hour to Ampere-second)
+    params.InputAmplitude = -1;
+    params.C_n_h = 4.1*abs(params.InputAmplitude);
+    params.C_n = params.C_n_h * 3600;                 
+
+    %%% system parameters %%%
+    % battery EMF
+    params.Voc = 3.3295;
+    % ohmic internal resistance
+    params.R0 = 0.0118;
+    % polarization resistance
+    params.R1 = 0.0120;
+    % polarization capacity
+    params.C1 = 3.31111e3;    
+    % Battery charging-discharging efficiency (for Li-ion=100%)
+    params.eta = 1;  
+
+    % noise characteristics
+    noise = 1;
+    params.percNoise = noise*5e-2;
+    params.NoisePwr = noise*5e-3;
+
+    % temperature
+    params.Temperature = 313.15;
+
+    % initial SOC
+    x10 = 0.5;
+    x20 = 0.01;    
+    
+    params.eps = 1;        
+    
+    %%%%%%% SETUP THE OBSERVER %%%%%
+    % out vars - observer
+    params.OutDim = 1;
+    params.OutDim_compare = [1];
+
+    % input dim - observer
+    params.dim_input = 1;                    
+       
+    % number of reference trajectories (under development)
+    params.Ntraj = 1;
+    
+    % state dimension
+    params.dim_state = 30;
+
+    % output dimension
+    params.OutDim = 1;
+    params.OutDim_compare = 1;
+
+    % dim input
+    params.dim_input = 1;
+    
+    % initial condition
+    params.X(1).val(:,1) = zeros(params.dim_state,1);
+    params.X(1).val(1:2,1) = [x10; x20];
+    
+    % same initial condition for all the trajectories (under development)
+    for traj=2:params.Ntraj
+        params.X(traj).val(:,1) = params.X(traj-1).val(:,1);
+    end
+    
+    % position in the state vector of the estimated parameters
+    params.estimated_params = [7:30];
+    
+    % which vars am I optimising
+    params.opt_vars = [1:2 8:10 12:14 16:18 20:22];
+    
+    % set the not optimised vars
+    tmp = 1:length(params.X(1).val(:,1));
+    tmp_idx = tmp;
+    for i=1:length(params.opt_vars)
+        tmp_idx = intersect(tmp_idx,find(tmp~=params.opt_vars(i)));
+    end
+    params.nonopt_vars = tmp_idx;
+    
+    % plot vars (used to plot the state estimation. When the parameters are
+    % too many, consider to use only the true state components)
+    params.plot_vars = 1:2;
+    params.plot_params = [4:6];%[7:14];
+    params.multi_traj_var = [1:2];
+
+    % add stuff
+    params.multistart = 0;
+    params.observed_state = [];
+    
+    % same initial condition for all the trajectories (under development)
+    for traj=2:params.Ntraj
+        params.X(traj).val(:,1) = params.X(traj-1).val(:,1);
+        params.X(traj).val(params.multi_traj_var,1) = params.X(traj-1).val(params.multi_traj_var,1) + abs(params.X(traj-1).val(params.multi_traj_var,1)).*randn(length(params.multi_traj_var),1);
+%         params.X(traj).val(params.multi_traj_var,1) = params.X(traj-1).val(params.multi_traj_var,1) + abs(params.X(traj-1).val(params.multi_traj_var,1)).*[1.5; 0.5; 1.5; 0.5];
+    end
+
+    %%% import data from sim %%%
+    sim = params_sim.out;
+    params.u_sim = sim.simout.u.Data';
+    params.y_sim = sim.simout.ECM_Vb_noise.Data';
+    params.y_true_sim = sim.simout.ECM_Vb.Data';
+    params.soc_sim = sim.simout.ECM_soc.Data';
+
+    % set the GT params (lin interp)
+    params.params_GT_sample = resample(params_sim.input_data.SOC,250,1);
+    params.params_GT_OCV = interp1(params_sim.input_data.SOC,params_sim.input_data.OCV,params.params_GT_sample);
+    params.params_GT_R0 = interp1(params_sim.input_data.SOC,params_sim.input_data.R0,params.params_GT_sample);
+    params.params_GT_R1 = interp1(params_sim.input_data.SOC,params_sim.input_data.R1,params.params_GT_sample);
+    params.params_GT_C1 = interp1(params_sim.input_data.SOC,params_sim.input_data.C1,params.params_GT_sample);
+
+    params = first_guess(params,params_sim);
+    
+end
\ No newline at end of file
diff --git a/Lib/models/battery/params_update_battery_tushar.m b/Lib/models/battery/params_update_battery_tushar.m
new file mode 100644
index 0000000..53f45ac
--- /dev/null
+++ b/Lib/models/battery/params_update_battery_tushar.m
@@ -0,0 +1,52 @@
+%% PARAMS_UPDATE_BATTERY
+% file: params_update_battery.m
+% author: Federico Oliva
+% date: 27/05/2022
+% description: this function updates the estimated parameters on a Van der 
+% Pol oscillator
+% INPUT:
+% params: structure with all the necessary parameters 
+% x: state vector
+% OUTPUT:
+% params_out: updated structure with the new model parameters
+function params_out = params_update_battery_tushar(params,x,t)
+    % assign params
+    params_out = params;          
+
+    params_out.Voc = x(3,:);
+    params_out.R0 = x(4,:);
+    params_out.R1 = x(5,:);
+    params_out.C1 = x(6,:);
+
+    params_out.alpha_Voc = x(7,:);
+    params_out.alpha_R0 = x(8,:);
+    params_out.alpha_R1 = x(9,:);
+    params_out.alpha_C1 = x(10,:);
+    
+    params_out.beta_Voc = x(11,:);
+    params_out.beta_R0 = x(12,:);
+    params_out.beta_R1 = x(13,:);
+    params_out.beta_C1 = x(14,:);
+    
+    params_out.gamma_Voc = x(15,:);
+    params_out.gamma_R0 = x(16,:);
+    params_out.gamma_R1 = x(17,:);
+    params_out.gamma_C1 = x(18,:);
+    
+    params_out.delta_Voc = x(19,:);
+    params_out.delta_R0 = x(20,:);
+    params_out.delta_R1 = x(21,:);
+    params_out.delta_C1 = x(22,:);
+    
+    params_out.eps_Voc = x(23,:);
+    params_out.eps_R0 = x(24,:);
+    params_out.eps_R1 = x(25,:);
+    params_out.eps_C1 = x(26,:);
+    
+    params_out.xi_Voc = x(27,:);
+    params_out.xi_R0 = x(28,:);
+    params_out.xi_R1 = x(29,:);
+    params_out.xi_C1 = x(30,:);
+    
+    
+end
\ No newline at end of file
diff --git a/Lib/models/model_init.m b/Lib/models/model_init.m
index 7fb5522..8dd28a4 100644
--- a/Lib/models/model_init.m
+++ b/Lib/models/model_init.m
@@ -11,11 +11,21 @@
 % params: structure with all the necessary parameter to the model
 function params = model_init(varargin)
 
+    % input law definition. Default is free evolution 
+    if any(strcmp(varargin,'addons'))
+        pos = find(strcmp(varargin,'addons'));
+        if ~isempty(varargin{pos+1})
+            addon = varargin{pos+1};
+        else
+            addon = [];
+        end 
+    end
+
     % get params_init.m script
     if any(strcmp(varargin,'params_init'))
         pos = find(strcmp(varargin,'params_init'));
         params_init = varargin{pos+1};
-        params = params_init();
+        params = params_init(addon);
     else
         params.X = 5;
     end
@@ -77,18 +87,7 @@
         params.StateDim = varargin{pos+1};
     else
         params.StateDim = params.dim_state;
-    end
-    
-    % get set of observed states. Default is 1
-    if any(strcmp(varargin,'ObservedState'))
-        pos = find(strcmp(varargin,'ObservedState'));
-        params.observed_state = varargin{pos+1};
-    else
-        params.observed_state = 1;
-    end
-    
-    % set the output dimensions from the observed state
-    params.OutDim = length(params.observed_state);
+    end               
     
     % get model if exists. Default is a 1 dimension asymptotically stable
     % system.
@@ -138,10 +137,6 @@
     else
         params.input_enable = 0;
     end
-    
-    % input dimension. Default is the whole state dimension. Whether to use
-    % it or not shall be explicited in the @model function
-    params.dim_input = params.StateDim;
 
     % input law definition. Default is free evolution 
     if any(strcmp(varargin,'input_law'))
@@ -151,7 +146,7 @@
         else
             params.input = @(x,params) 0;
         end 
-    end
+    end    
                
     % remark: the params.Ntraj variable describes on how many different
     % trajectories the MHE is run. This makes sense in the control design
@@ -175,41 +170,29 @@
             params.perc = zeros(params.StateDim,params.Ntraj);
             
             % randomly define the percentage (bool flag, see below)
-            randflag = 0;
-            
-            % if case: random perturbation percentage - non optimised vars
-            if randflag
-                params.perc(params.nonopt_vars,traj) = 1*randn(1,length(params.nonopt_vars))*5e-1;
-            else
-                params.perc(params.nonopt_vars,traj) = 1*ones(1,length(params.nonopt_vars))*6e-1;
-            end
+            randflag = 1; 
+            noise_std = 0*5e-1;
 
-            % % if case: random perturbation percentage - optimised vars
+            % if case: random perturbation percentage - optimised vars            
             if randflag
-                params.perc(params.opt_vars,traj) = 1*randn(1,length(params.opt_vars));
+                params.perc(params.multi_traj_var,traj) = 1*randn(1,length(params.multi_traj_var))*noise_std;
             else
-                params.perc(params.opt_vars,traj) = 1*ones(1,length(params.opt_vars))*6e-1;
-            end
+                params.perc(params.multi_traj_var,traj) = 1*ones(1,length(params.multi_traj_var))*noise_std;
+            end            
+            
 
             % final setup on perc
-            params.perc = 1*params.perc;
-            params.X_est(traj).val(:,1) = init.*(1 + params.noise*params.perc(:,traj).*ones(params.StateDim,1)) + params.noise*params.noise_std.*randn(params.StateDim,1);
-        else
-        
-            %%%% PERTURBATION ON X0 WITH RANDOM NOISE %%%%
-            % define noise boundaries (uniform distribution)
-            params.bound_delta_x = params.noise*1e-1*[-1,1]*1;
-            params.bound_delta_x_dot = params.noise*1e-1*[-1,1]*1;
-
-            % define the perturbation on the initial condition
-            noise = [unifrnd(params.bound_delta_x(1),params.bound_delta_x(2),2,1); unifrnd(params.bound_delta_x_dot(1),params.bound_delta_x_dot(2),2,1)];
-
-            % final setup on initial condition
+            params.perc = 1*params.perc;            
             params.X_est(traj).val(:,1) = init;
-            params.X_est(traj).val(1:params.dim_state,1) = params.X(traj).val(1:params.dim_state,1) + params.noise*noise;
+            
+            if params.noise                                            
+                % around init
+                params.X_est(traj).val(params.multi_traj_var,1) = init(params.multi_traj_var).*(1 + params.noise*params.perc(params.multi_traj_var,traj).*ones(length(params.multi_traj_var),1)) + ...
+                                                                  params.noise*noise_std.*randn(length(params.multi_traj_var),1);                                
+            end
         end
 
         
     end
     
-end
\ No newline at end of file
+end
diff --git a/Lib/models/model_reference.m b/Lib/models/model_reference.m
new file mode 100644
index 0000000..0decfb2
--- /dev/null
+++ b/Lib/models/model_reference.m
@@ -0,0 +1,56 @@
+%% MODEL_REFERENCE
+% file: model_reference.m
+% author: Federico Oliva
+% date: 22/06/2022
+% description: this function describes the dynamics equation to be used as
+% reference in the control design
+% INPUT:
+% t: time instant
+% x: state vector
+% params: structure with all the necessary parameters 
+% obs: observer class instance (may be not used)
+% OUTPUT:
+% x_dot: dynamics equations
+function x_dot = model_reference(t, x, params, obs)
+
+    % init the dynamics 
+    x_dot = zeros(length(x),1);
+    
+    % compute the control
+    tdiff = obs.setup.time-t;   
+    pos = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+    % check length of measurements
+    pos = max(1,min(pos,size(obs.init.Y_full_story(obs.init.traj).val,3)));
+    drive_out = drive(obs,obs.init.X(obs.init.traj).val(:,pos),x,obs.init.Y_full_story(obs.init.traj).val(:,:,max(1,pos-1):pos),pos);
+    params.u = params.input(t,drive_out,params,obs);      
+    
+    % save input
+    obs.init.input_story(obs.init.traj).val(:,pos) = params.u;
+    
+    % Plant - to be estimated
+    Ap = [0 1; params.a0est params.a1est];
+    Bp = [params.b0est; params.b1est];
+    
+    % Plant - true one
+    Ap_t = [params.A1 params.A2; params.A3 params.A4];
+    Bp_t = [params.B1; params.B2]; 
+    
+    % Control
+    Ac = [0 1; params.a0 params.a1];
+    Bc = [params.b0; params.b1];     
+    
+    % u1 = uc (input to plant - ref track)
+    % u2 = ec (input to controller - ref track)
+    % u3 = ur (input to reference model - ref track)
+    
+    %%% model dynamics %%%           
+    % estimated plant dynamics - reference tracking
+    x_dot(1:2,:) = Ap*x(1:2,:) + Bp*(params.u(1,:));
+    % true plant dynamics - reference tracking
+    x_dot(3:4,:) = Ap_t*x(3:4,:) + Bp_t*(params.u(1,:));
+    % controller dynamics - reference tracking
+    x_dot(5:6,:) = Ac*x(5:6,:) + Bc*(params.u(2,:));
+    % reference model dynamics - reference tracking (useless here)
+    x_dot(7,:) = params.alpha*x(7,:)+abs(params.alpha)*params.u(3,:);
+        
+end
\ No newline at end of file
diff --git a/Lib/models/rover/J_dist.m b/Lib/models/rover/J_dist.m
new file mode 100644
index 0000000..dc91fe5
--- /dev/null
+++ b/Lib/models/rover/J_dist.m
@@ -0,0 +1,13 @@
+%%
+function J = J_dist(x,Pa,D)
+
+    % init
+    N = length(D);
+    J = 0;
+
+    % 
+    for i=1:N
+        J = J + abs(sqrt((x(1)-Pa(1,i))^2 + (x(2)-Pa(2,i))^2 + (x(3)-Pa(3,i))^2) - D(i));
+    end
+
+end
\ No newline at end of file
diff --git a/Lib/models/rover/armesto/model_armesto.m b/Lib/models/rover/armesto/model_armesto.m
new file mode 100644
index 0000000..528f209
--- /dev/null
+++ b/Lib/models/rover/armesto/model_armesto.m
@@ -0,0 +1,44 @@
+%% MODEL_ROVER
+% file: model_rover.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function describes the dynamics equation of a rover
+% t: time instant
+% x: state vector
+% params: structure with all the necessary parameters 
+% obs: observer class instance (may be not used)
+% OUTPUT:
+% x_dot: dynamics equations
+function [x_dot, x] = model_armesto(tspan,x,params,obs)
+
+    % init the dynamics 
+    x_dot = zeros(length(x),1);
+
+    % compute the time index
+    for i=1:length(tspan)
+        tdiff = obs.setup.time-tspan(i);   
+        pos(i) = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+        pos(i) = max(1,pos(i));        
+    end    
+    
+    % compute the control
+    params.u = params.input(tspan,x,params,obs);    
+    obs.init.input_story_ref(obs.init.traj).val(:,pos(1)) = params.u(:,1);    
+    obs.init.input_story(obs.init.traj).val(:,pos(1)) = params.u(:,1);           
+
+    % Skew matrix
+    q = x(params.pos_quat);
+    S = [q(1) -q(2) -q(3) -q(4); ...
+         q(2) q(1) q(4) -q(3); ...
+         q(3) -q(4) q(1) q(2); ...
+         q(4) q(3) -q(2) q(1)];
+    
+    % model dynamics - fc    
+    x_dot(params.pos_p) = x(params.pos_p) + params.Ts*x(params.pos_v) + 0.5*params.Ts^2*x(params.pos_acc);
+    x_dot(params.pos_v) = x(params.pos_v) + params.Ts*x(params.pos_acc);
+    x_dot(params.pos_acc) = x(params.pos_acc);
+    x_dot(params.pos_bias) = x(params.pos_bias);                
+
+    % all the remaining are the anchors
+    
+end
\ No newline at end of file
diff --git a/Lib/models/rover/armesto/model_armesto_reference.m b/Lib/models/rover/armesto/model_armesto_reference.m
new file mode 100644
index 0000000..285083f
--- /dev/null
+++ b/Lib/models/rover/armesto/model_armesto_reference.m
@@ -0,0 +1,63 @@
+%% MODEL_ROVER
+% file: model_rover.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function describes the dynamics equation of a rover
+% t: time instant
+% x: state vector
+% params: structure with all the necessary parameters 
+% obs: observer class instance (may be not used)
+% OUTPUT:
+% x_dot: dynamics equations
+function [x_dot, x] = model_armesto_reference(tspan,x,params,obs)
+
+    % init the dynamics 
+    x_dot = zeros(length(x),1);
+
+    % compute the time index
+    for i=1:length(tspan)
+        tdiff = obs.setup.time-tspan(i);   
+        pos(i) = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+        pos(i) = max(1,pos(i));        
+    end    
+    
+    % compute the control
+    params.u = params.input(tspan,x,params,obs);    
+    obs.init.input_story_ref(obs.init.traj).val(:,pos(1)) = params.u(:,1);    
+    obs.init.input_story(obs.init.traj).val(:,pos(1)) = params.u(:,1);           
+
+    % Skew matrix
+    q = x(params.pos_quat);
+    S = [q(1) -q(2) -q(3) -q(4); ...
+         q(2) q(1) q(4) -q(3); ...
+         q(3) -q(4) q(1) q(2); ...
+         q(4) q(3) -q(2) q(1)];
+    
+    % model dynamics - fc    
+    x_dot(params.pos_p) = x(params.pos_p) + params.Ts*x(params.pos_v) + 0.5*params.Ts^2*x(params.pos_acc) + 1/6*params.Ts^2*x(params.pos_jerk);
+    x_dot(params.pos_v) = x(params.pos_v) + params.Ts*x(params.pos_acc) + 0.5*params.Ts^2*x(params.pos_jerk);
+    x_dot(params.pos_acc) = x(params.pos_acc) + params.Ts*( x(params.pos_jerk) +  cross(x(params.pos_alpha),x(params.pos_v)) + cross(x(params.pos_omega),x(params.pos_acc)));
+    x_dot(params.pos_bias) = x(params.pos_bias) + params.Ts*x(params.pos_bias_dyn);        
+
+    % model dynamics - quaternion
+    tmp = 0.5*params.Ts*x(params.pos_omega) + 0.25*params.Ts^2*x(params.pos_alpha);
+    den = x(params.pos_omega)+0.5*params.Ts*x(params.pos_alpha);
+    vec = [cos(norm(tmp)); ...
+           sin(norm(tmp)/norm(den))*(den)];
+    if any(isnan(vec))
+        x_dot(params.pos_quat) = [1 0 0 0]';
+    else
+        x_dot(params.pos_quat) = S*vec;
+    end
+
+    % model dynamics - angular velocity
+    x_dot(params.pos_omega) = x(params.pos_omega) + params.Ts*x(params.pos_alpha);
+    
+    % noise dynamics
+    x_dot(params.pos_jerk) = 0*x(params.pos_jerk) + params.jerk_enable*ones(3,1)*params.Ts*chirp(tspan(1),1,5,100)*cos(2*tspan(1))^2;
+    x_dot(params.pos_alpha) = 0*x(params.pos_alpha) + params.alpha_enable*ones(3,1)*params.Ts*chirp(tspan(1),1,5,100)*1e1*sin(2*tspan(1))^2;  
+    x_dot(params.pos_bias_dyn) = 0*x(params.pos_bias_dyn) + params.bias_dyn_enable*params.Ts*ones(3,1)*sin(tspan(1));
+
+    % all the remaining are the anchors
+    
+end
\ No newline at end of file
diff --git a/Lib/models/rover/armesto/params_armesto.m b/Lib/models/rover/armesto/params_armesto.m
new file mode 100644
index 0000000..79c2150
--- /dev/null
+++ b/Lib/models/rover/armesto/params_armesto.m
@@ -0,0 +1,179 @@
+%% PARAMS_ROVER
+% file: params_rover.m
+% author: Federico Oliva
+% date: 30/11/2022
+% description: this function initialises the parameters for a double
+% integrator
+% INPUT: none
+% OUTPUT:
+% params: structure with all the necessary parameters
+function params = params_armesto    
+    
+    % control parameters   
+    params.Ts = 1e-2;
+    params.wnx = [0.4];
+    params.wny = [0.4];
+    params.Ax = -[0.5 1];
+    params.Ay = -[0.5 1];     
+    params.Ax_tot = sum(params.Ax);
+    params.Ay_tot = sum(params.Ay);
+    params.phi = [0 pi/2];
+    params.rhox = 0.01;
+    params.rhoy = 0.01;
+    % vines
+    params.freq_u = 48;    
+    params.amp_ux = -1/2;
+    params.amp_uy = -1/6;
+    params.Ku = [10 10];
+
+    % number of reference trajectories (under development)
+    params.Ntraj = 1;        
+    
+    % space dimension        
+
+    % multistart
+    params.multistart = 0;
+
+    % observer params        
+   
+%     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+    params.dim_state = 28;        
+    params.pos_p = [1:3];           % see mode_rover.m
+    params.pos_v = [4:6];           % see mode_rover.m    
+    params.pos_acc = [7:9];         % see mode_rover.m    
+    params.pos_bias = [10:12];      % see mode_rover.m    
+    params.pos_quat = [13:16];      % see mode_rover.m    
+    params.pos_omega = [17:19];     % see mode_rover.m    
+    params.pos_jerk = [20:22];      % see mode_rover.m    
+    params.pos_alpha = [23:25];     % see mode_rover.m    
+    params.pos_bias_dyn = [26:28];  % see mode_rover.m   
+    params.pos_fc = [params.pos_p params.pos_v params.pos_acc params.pos_bias];
+    params.dim_state_est = numel(params.pos_fc);
+
+    % input dim
+    params.dim_input = 3;           % input on each dimension + orientation
+
+    % output dim
+    params.OutDim = length([params.pos_p params.pos_quat params.pos_acc params.pos_omega]);  
+    params.observed_state = [params.pos_p params.pos_quat params.pos_acc params.pos_omega];     
+    params.OutDim_compare = params.OutDim;
+    params.pos_p_out = [1:3];
+    params.pos_quat_out = [4:7];
+    params.pos_acc_out = [8:10];
+    params.pos_omega_out = [11:13];
+
+    % sampling
+    params.IMU_samp = 1;
+    params.CAM_samp = 1;
+
+    % memory
+    params.last_noise = zeros(params.Ntraj,params.OutDim);
+    params.last_CAM_pos = zeros(params.Ntraj,numel(params.pos_p_out));
+    params.last_CAM_quat = zeros(params.Ntraj,numel(params.pos_quat_out));
+    params.last_IMU_acc = zeros(params.Ntraj,numel(params.pos_acc_out));
+    params.last_IMU_omega = zeros(params.Ntraj,numel(params.pos_omega_out));
+               
+
+    % noise (on distances + acceleration)
+    params.noise_mat = 0*ones(params.OutDim,2);
+    % bias
+    params.noise_mat(params.pos_p_out,1) = 0*1e-2;          % noise on cam pos - bias 
+    params.noise_mat(params.pos_quat_out,1) = 0*1e-2;       % noise on cam quat - bias 
+    params.noise_mat(params.pos_acc_out,1) = 0*1e-3;        % noise on IMU acc - bias 
+    params.noise_mat(params.pos_omega_out,1) = 0*1e-3;      % noise on IMU gyro - bias 
+    % sigma
+    params.noise_mat(params.pos_p_out,2) = 0*1e-3;          % noise on cam pos - sigma 
+    params.noise_mat(params.pos_quat_out,2) = 0*1e-2;       % noise on cam quat - sigma
+    params.noise_mat(params.pos_acc_out,2) = 0*1e-4;        % noise on IMU acc - sigma
+    params.noise_mat(params.pos_omega_out,2) = 0*1e-2;      % noise on IMU gyro - sigma
+
+    % enable noise
+    params.EKF = 1;
+    params.jerk_enable = 1;
+    params.alpha_enable = 0;
+    params.bias_dyn_enable = 1;
+    params.pos_biased_out = params.pos_p_out;
+
+    %%% noise matrices
+    % measurement noise
+    params.R = diag([params.noise_mat(params.pos_p_out).^2*eye(3), ...         % CAM POS
+                     params.noise_mat(params.pos_quat_out).^2*eye(4), ...      % CAM QUAT
+                     params.noise_mat(params.pos_acc_out).^2*eye(3), ...       % IMU ACC
+                     params.noise_mat(params.pos_omega_out).^2*eye(3), ...     % IMU OMEGA
+        ]);      
+    
+    % process noise - centripetal model
+    params.Q = diag([1e0 1e0 1e0,    ... % JERK
+                     1e0 1e0 1e0,    ... % ALPHA
+                     1e5 1e5 1e5,    ... % BIAS VEL
+        ]);
+
+    % EKF covariance matrix
+    for traj=1:params.Ntraj
+        params.Phat(traj).val(1,:,:) = 1e0*eye(params.dim_state);
+    end
+
+    % observer stuff
+    params.time_J = [];
+    params.d_true = zeros(params.Nanchor,1);
+    params.d_noise = zeros(params.Nanchor,1);
+    params.d_est = zeros(params.Nanchor,1);
+    
+    % initial condition
+    params.X(1).val(:,1) = 1*[  ...
+                              0;0;0;        ... % pos
+                              0;0;0;        ... % vel
+                              0;0;0;        ... % acc                
+                              0;0;0;        ... % bias
+                              1;0;0;0;      ... % quat
+                              0;0;0;        ... % omega
+                              0;0;0;        ... % jerk
+                              0;0;0;        ... % alpha
+                              0;0;0;        ... % bias vel
+                              ];
+    
+    % position in the state vector of the estimated parameters
+    params.estimated_params = [params.pos_bias];
+    
+    % which vars am I optimising
+    params.opt_vars = [];
+    
+    % set the not optimised vars
+    tmp = 1:length(params.X(1).val(:,1));
+    tmp_idx = tmp;
+    for i=1:length(params.opt_vars)
+        tmp_idx = intersect(tmp_idx,find(tmp~=params.opt_vars(i)));
+    end
+    params.nonopt_vars = tmp_idx;
+
+    % multistart
+    if params.multistart
+        Npoints = 3;
+        decades = linspace(0,4,Npoints);
+    
+        for i=1:Npoints 
+            params.MULTISTART(i,:) = repmat(decades(i),1,length(params.opt_vars));   
+            params.MULTISTART(i,end-1:end) = params.X(1).val(params.opt_vars(end-1:end),1);
+        end 
+        params.tpoints = CustomStartPointSet(params.MULTISTART(:,1:end));
+        params.X(1).val(params.opt_vars(1:end),1) = params.MULTISTART(1,1:end);
+    end
+
+
+    % same initial condition for all the trajectories (under development)
+    params.multi_traj_var = [params.pos_p params.pos_v params.pos_acc params.pos_bias];     
+    for traj=2:params.Ntraj
+        params.X(traj).val(:,1) = params.X(1).val(:,1);
+
+        % random
+        params.X(traj).val(params.multi_traj_var,1) = params.X(1).val(params.multi_traj_var,1).*(1 + 0*5e-1*randn(length(params.multi_traj_var),1));
+        
+    end    
+    
+    % plot vars (used to plot the state estimation. When the parameters are
+    % too many, consider to use only the true state components)
+    params.plot_vars = [params.pos_p params.pos_v params.pos_acc];
+    params.plot_params = [params.pos_bias params.pos_jerk params.pos_alpha params.pos_bias_dyn];
+    params.dim_out_plot = params.OutDim;       
+        
+end
diff --git a/Lib/models/rover/armesto/params_update_armesto.m b/Lib/models/rover/armesto/params_update_armesto.m
new file mode 100644
index 0000000..a60c98f
--- /dev/null
+++ b/Lib/models/rover/armesto/params_update_armesto.m
@@ -0,0 +1,16 @@
+%% PARAMS_UPDATE_ROVER
+% file: params_update_rover.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function updates the estimated parameters on a rover
+% INPUT:
+% params: structure with all the necessary parameters 
+% x: state vector
+% OUTPUT:
+% params_out: updated structure with the new model parameters
+function params_out = params_update_armesto(params,x)
+
+    % assign params
+    params_out = params;         
+    
+end
\ No newline at end of file
diff --git a/Lib/models/rover/model_observer.m b/Lib/models/rover/model_observer.m
new file mode 100644
index 0000000..ed40479
--- /dev/null
+++ b/Lib/models/rover/model_observer.m
@@ -0,0 +1,38 @@
+%% MODEL_ROVER
+% file: model_rover.m
+% author: Federico Oliva
+% date: 05/12/2022
+% description: this function describes the dynamics equation of a rover
+% t: time instant
+% x: state vector
+% y: measures (filtred?)
+% params: structure with all the necessary parameters 
+% obs: observer class instance (may be not used)
+% OUTPUT:
+% x_dot: dynamics equations
+function x_dot = model_observer(tspan,x,y,params,obs)
+
+    % init the dynamics 
+    x_dot = zeros(length(x),1);
+
+    % compute the time index
+    for i=1:length(tspan)
+        tdiff = obs.setup.time-tspan(i);   
+        pos(i) = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+        pos(i) = max(1,pos(i));        
+    end    
+    
+    % compute the control
+    params.u = params.input(tspan,x,params);
+           
+    alpha = [-0.5 -0.5 -0.2 -0.2];     
+    % observer dynamics simplified
+    x_dot(1) =  x(3) + alpha(1)*(x(1)-x(5));
+    x_dot(2) =  x(4) + alpha(2)*(x(2)-x(6));    
+    x_dot(3) = alpha(3)*x(3) + y(params.pos_acc(1));
+    x_dot(4) = alpha(4)*x(4) + y(params.pos_acc(2));
+    x_dot(5) = 0;
+    x_dot(6) = 0;
+    
+    
+end
\ No newline at end of file
diff --git a/Lib/models/rover/model_rover.m b/Lib/models/rover/model_rover.m
new file mode 100644
index 0000000..60b9474
--- /dev/null
+++ b/Lib/models/rover/model_rover.m
@@ -0,0 +1,129 @@
+% MODEL_ROVER
+% file: model_rover.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function describes the dynamics equation of a rover
+% t: time instant
+% x: state vector
+% params: structure with all the necessary parameters 
+% obs: observer class instance (may be not used)
+% OUTPUT:
+% x_dot: dynamics equations
+function [x_dot, x] = model_rover(tspan,x,params,obs)
+
+    % init the dynamics 
+    x_dot = zeros(length(x),1);
+
+    % compute the time index
+    for i=1:length(tspan)
+        tdiff = obs.setup.time-tspan(i);   
+        pos(i) = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+        pos(i) = max(1,pos(i));        
+    end    
+    
+    % compute the control
+    % use input from reference
+    params.u = obs.init.input_story_ref(obs.init.traj).val(:,max(1,pos(1)));
+    obs.init.input_story(obs.init.traj).val(:,pos(1)) = params.u(:,1);   
+
+    %%%%%%%%%%% HYBRID OBSERVER MODEL %%%%%%%%%%%%%
+
+    if (params.hyb && ~params.EKF) && ~params.dryrun
+
+        % meas available
+        y = obs.init.Y_full_story(obs.init.traj).val(1,:,pos(1));
+        a = y(params.pos_acc_out);        
+    
+        % Jump - only on the UWB
+        if (mod(pos(1),params.UWB_samp) == 0) && (~params.EKF)
+    
+            % adjacency matrix
+            for dim=1:params.space_dim
+                Pa(dim,:) = x(params.pos_anchor(dim):params.space_dim:params.pos_anchor(end));            
+            end
+    
+            %%% TEST %%%
+            p_jump = obs.init.params.p_jump(obs.init.traj).val(:,pos(1)/params.UWB_samp);
+            p_jump_der = obs.init.params.p_jump_der(obs.init.traj).val(:,pos(1)/params.UWB_samp);
+
+            % gamma
+            normx = norm(x(params.pos_p(1)));
+            normy = norm(x(params.pos_p(2)));
+            normz = norm(x(params.pos_p(3)));
+            gamma(1) = params.theta(1) + 0*params.theta(2)*normx;
+            gamma(2) = params.theta(1) + 0*params.theta(2)*normy;
+            gamma(3) = params.theta(1) + 0*params.theta(2)*normz;
+            
+            % jump map - x
+            x(1) = gamma(1)*x(1) + (1-gamma(1))*p_jump(1);
+%             x(2) = params.theta(3)*x(2) + (1-params.theta(3))*p_jump_der(1);
+%             x(3) = params.theta(4)*x(3);
+%             x(4) = params.theta(5)*x(4);
+    
+            % jump map - y
+            x(6) = gamma(2)*x(6) + (1-gamma(2))*p_jump(2);
+%             x(7) = params.theta(3)*x(7) + (1-params.theta(3))*p_jump_der(2);
+%             x(8) = params.theta(4)*x(8);
+%             x(9) = params.theta(5)*x(9);
+
+            % jump map - z
+            x(11) = gamma(3)*x(11) + (1-gamma(3))*p_jump(3);
+%             x(12) = params.theta(3)*x(12) + (1-params.theta(3))*p_jump_der(3);
+%             x(13) = params.theta(4)*x(13);
+%             x(14) = params.theta(5)*x(14);
+        end
+    
+        %%%% OBSERVER DYNAMICS %%%
+        % model dynamics
+        % x axis
+        x_dot(1) = x(2);
+        x_dot(2) = x(3) - params.alpha(1)*x(2) + 1*params.alpha(2)*abs(x(3))*x(3);
+        x_dot(3) = x(4) + params.beta(1)*a(1);
+        x_dot(4) = -params.C(1)*x(3) -params.C(2)*x(4) + params.beta(2)*a(1);
+    
+        % y axis
+        x_dot(6) = x(7);
+        x_dot(7) = x(8) - params.alpha(1)*x(7) + 1*params.alpha(2)*abs(x(8))*x(8);
+        x_dot(8) = x(9) + params.beta(1)*a(2);
+        x_dot(9) = -params.C(1)*x(8) -params.C(2)*x(9) + params.beta(2)*a(2);  
+
+        % z axis
+        x_dot(11) = x(12);
+        x_dot(12) = x(13) - params.alpha(1)*x(12) + 1*params.alpha(2)*abs(x(13))*x(13);
+        x_dot(13) = x(14) + params.beta(1)*a(3);
+        x_dot(14) = -params.C(1)*x(13) -params.C(2)*x(14) + params.beta(2)*a(3);  
+
+    %%%%%%%%%%%%% EKF MODEL %%%%%%%%%%%%
+    elseif (params.EKF && ~params.hyb) && ~params.dryrun
+
+        % model dynamics
+        % x axis
+        x_dot(1) = x(2);    
+        x_dot(2) = x(3);
+        x_dot(3) = 0;
+        
+        % y axis
+        x_dot(6) = x(7);    
+        x_dot(7) = x(8);   
+        x_dot(8) = 0;
+
+        % z axis
+        x_dot(11) = x(12);    
+        x_dot(12) = x(13);   
+        x_dot(13) = 0;
+
+    elseif params.dryrun
+
+    %%%%%%%%%%%%% REFERENCE MODEL %%%%%%%%%
+        x_dot = obs.setup.model_reference(tspan,x,params,obs);
+
+    else
+
+    %%%%%%%%%%%%% ERROR %%%%%%%%%
+        error('what do you wanna integrate mate?')
+
+    end
+
+
+    
+end
\ No newline at end of file
diff --git a/Lib/models/rover/model_rover_EKF.m b/Lib/models/rover/model_rover_EKF.m
new file mode 100644
index 0000000..52996b0
--- /dev/null
+++ b/Lib/models/rover/model_rover_EKF.m
@@ -0,0 +1,39 @@
+%% MODEL_ROVER
+% file: model_rover.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function describes the dynamics equation of a rover
+% t: time instant
+% x: state vector
+% params: structure with all the necessary parameters 
+% obs: observer class instance (may be not used)
+% OUTPUT:
+% x_dot: dynamics equations
+function [x_dot, x] = model_rover_EKF(tspan,x,params,obs)
+
+    % init the dynamics 
+    x_dot = zeros(length(x),1);
+
+    % compute the time index
+    for i=1:length(tspan)
+        tdiff = obs.setup.time-tspan(i);   
+        pos(i) = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+        pos(i) = max(1,pos(i));        
+    end    
+    
+    % compute the control     
+    params.u = obs.init.input_story_ref(obs.init.traj).val(:,max(1,pos(1)));  
+    obs.init.input_story(obs.init.traj).val(:,pos(1)) = params.u(:,1);           
+    
+    % model dynamics
+    % x axis
+    x_dot(1) = x(2);    
+    x_dot(2) = x(3);
+    x_dot(3) = 0;
+    
+    % y axis
+    x_dot(5) = x(6);    
+    x_dot(6) = x(7);   
+    x_dot(7) = 0;
+    
+end
\ No newline at end of file
diff --git a/Lib/models/rover/model_rover_reference.m b/Lib/models/rover/model_rover_reference.m
new file mode 100644
index 0000000..931bcd7
--- /dev/null
+++ b/Lib/models/rover/model_rover_reference.m
@@ -0,0 +1,51 @@
+%% MODEL_ROVER
+% file: model_rover.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function describes the dynamics equation of a rover
+% t: time instant
+% x: state vector
+% params: structure with all the necessary parameters 
+% obs: observer class instance (may be not used)
+% OUTPUT:
+% x_dot: dynamics equations
+function [x_dot, x] = model_rover_reference(tspan,x,params,obs)
+
+    % init the dynamics 
+    x_dot = zeros(length(x),1);
+
+    % compute the time index
+    for i=1:length(tspan)
+        tdiff = obs.setup.time-tspan(i);   
+        pos(i) = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+        pos(i) = max(1,pos(i));        
+    end    
+    
+    % compute the control
+    params.u = params.input(tspan,x,params,obs);    
+    obs.init.input_story_ref(obs.init.traj).val(:,pos(end)) = params.u(1:3,1);
+    obs.init.reference_story(obs.init.traj).val(:,pos(end)) = params.u(4,1);
+
+    % process noise
+    w = params.jerk_enable*params.Ts*randn(2*params.space_dim,1);
+    obs.init.params.proc_noise_story(obs.init.traj).val(:,pos(1)) = w; 
+    
+    % model dynamics
+    % x axis
+    x_dot(1) = x(2);
+    x_dot(2) = x(3) + params.u(1,1) + 0*w(1);
+    x_dot(5) = 0*w(4);
+    
+    % y axis
+    x_dot(6) = x(7);
+    x_dot(7) = x(8) + params.u(2,1) + 0*w(2);
+    x_dot(10) = 0*w(5);
+
+    % y axis
+    x_dot(11) = x(12);
+    x_dot(12) = x(13) + params.u(3,1) + 0*w(3) + params.g;
+    x_dot(15) = 0*w(6);
+
+    % all the remaining are the anchors
+    
+end
\ No newline at end of file
diff --git a/Lib/models/rover/nonlcon_fcn_rover.m b/Lib/models/rover/nonlcon_fcn_rover.m
new file mode 100644
index 0000000..0e12342
--- /dev/null
+++ b/Lib/models/rover/nonlcon_fcn_rover.m
@@ -0,0 +1,42 @@
+%% fcn
+function [c, ceq] = nonlcon_fcn_rover(xopt,xnonopt,obs)    
+    
+    % create full state vector
+    x = zeros(obs.setup.dim_state,1);
+    x(obs.setup.opt_vars) = xopt;
+    x(obs.setup.nonopt_vars) = xnonopt(1).val;  
+    
+    % tolerance
+    tol = 1e0*obs.init.myoptioptions.ConstraintTolerance;
+
+    % init
+    c = [];    
+
+%     tspan_pos = [1, nonzeros(obs.init.Y_space_full_story)'];
+%     tspan = obs.setup.time(tspan_pos(1):tspan_pos(end));
+%     X = obs.setup.ode(@(t,x)obs.setup.model(t, x, obs.init.params, obs), tspan , x, obs.setup.odeset);
+%     X = X.y(:,obs.init.Y_space_full_story(2:end));
+
+%     normx = vecnorm(X(obs.init.params.pos_p(1),:),2,1);
+%     normy = vecnorm(X(obs.init.params.pos_p(2),:),2,1);
+%     gamma(:,1) = x(obs.init.params.pos_Gamma(3))+ x(obs.init.params.pos_Gamma(4))*normx;
+%     gamma(:,2) = x(obs.init.params.pos_Gamma(3))+ x(obs.init.params.pos_Gamma(4))*normy;
+    theta1 = x(obs.init.params.pos_Gamma(3));
+% 
+%     theta_constr_up = [gamma(:,1); gamma(:,2); theta2] - 1;
+%     theta_constr_down = -[gamma(:,1); gamma(:,2); theta2];
+    
+    theta_constr_up = [theta1] - 1;
+    theta_constr_down = -[theta1];
+    c = [c; theta_constr_up; theta_constr_down];
+
+    % negative poles for filter
+    A = [0 1; -x(obs.init.params.pos_Gamma(1:2))'];
+    C_eig = eig(A);
+    c = [c; C_eig + tol];
+    
+            
+    % cons
+    ceq = [];
+
+end
\ No newline at end of file
diff --git a/Lib/models/rover/params_rover.m b/Lib/models/rover/params_rover.m
new file mode 100644
index 0000000..332fbed
--- /dev/null
+++ b/Lib/models/rover/params_rover.m
@@ -0,0 +1,283 @@
+%% PARAMS_ROVER
+% file: params_rover.m
+% author: Federico Oliva
+% date: 30/11/2022
+% description: this function initialises the parameters for a double
+% integrator
+% INPUT: none
+% OUTPUT:
+% params: structure with all the necessary parameters
+function params = params_rover
+
+    % system parameters
+    params.m = 1;
+    params.eps = 5;    
+    params.Nanchor = 4;
+    params.g = 0*0.1;
+    
+    % control parameters
+    % 2nd order system
+%     params.wnx = [0.2 1];
+%     params.wny = [0.2 1];
+    params.wnx = [0.4];
+    params.wny = [0.9];
+    params.Ax = -[0.5 1];
+    params.Ay = -[0.5 1];     
+    params.Ax_tot = sum(params.Ax);
+    params.Ay_tot = sum(params.Ay);
+    params.phi = [0 pi/2];
+    params.rhox = 0.01;
+    params.rhoy = 0.01;
+    % vines
+    params.freq_u = 48;    
+    params.amp_ux = -1/3;
+    params.amp_uy = -1/3;
+    params.Ku = [10 10];    
+    params.Kdu = [0 0];      
+    params.Kz = [9000 190];
+    params.Kff = [0 0 0];
+
+    % number of reference trajectories (under development)
+    params.Ntraj = 1;
+
+    % control error derivative
+    params.wlen_err = 4;
+    params.buflen_err = 10;
+    params.dim_err = 1;
+    params.err_scale = 1;
+    for traj = 1:params.Ntraj        
+        params.err(traj).val = [];
+        params.err_der(traj).val = [];
+        params.err_int(traj).val = zeros(params.dim_err,1);
+        params.err_der_buffer(traj).val = zeros(params.dim_err,params.buflen_err);
+        params.err_der_counter(traj).val = 0;
+    end 
+    
+    % different omega for trajectories
+    for traj = 2:params.Ntraj
+        params.wnx(traj) = params.wnx(1)*(1+rand());        
+        params.wny(traj) = params.wny(1)*(1+rand());
+    end
+    
+    % state dimension
+    params.space_dim = 3;   % 2D or 3D space for the rover 
+
+    % anchor stuff
+    an_dp = 2.5;
+    an_dz = 2;
+
+    %%% gaussian stuff %%%
+    ds = 1e-2*params.err_scale;
+    [params.X_gauss, params.Y_gauss] = meshgrid(-an_dp:ds:an_dp, -an_dp:ds:an_dp);
+    params.A_gauss = 0.5;
+    params.sigma_gauss = 1;
+    params.hill(1,:) = [-an_dp*1.5 -an_dp*1.5];
+    params.hill(2,:) = [-an_dp*1.5 an_dp*1.5];
+    params.hill(3,:) = [an_dp*1.5 an_dp*1.5];
+    params.hill(4,:) = [an_dp*1.5 -an_dp*1.5];
+    params.G_gauss = [];
+    for hills=1:size(params.hill,1)
+        try
+            params.G_gauss = 1*params.G_gauss + 1*params.A_gauss*exp(-1/(params.sigma_gauss^2)*((params.Y_gauss-params.hill(hills,2)/2).^2 + (params.X_gauss-params.hill(hills,1)/2).^2));
+        catch
+            params.G_gauss = 1*params.A_gauss*exp(-1/(params.sigma_gauss^2)*((params.Y_gauss-params.hill(hills,2)/2).^2 + (params.X_gauss-params.hill(hills,1)/2).^2));
+        end
+    end
+
+    % multistart
+    params.multistart = 0;
+
+    % observer params    
+    params.alpha = 0*[1 1];
+    params.beta = 0*[1 -100.1];
+    params.C = 0*[10 100.1];
+    params.theta = 0*[1 0 0 0 0];
+
+    % observer params    
+%     params.alpha = 1*[-0.0067    6.4999];
+%     params.beta = 1*[1.0000   83.8996];
+%     params.C = 1*[243.9468  -83.8996];
+%     params.theta = 1*[0.9328   -0.1371    0.9731   -0.1163    7.3477];    
+   
+    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+    % hyb obs parameters
+    params.dim_Gamma = length(params.C) + length(params.theta) +length(params.beta) + length(params.alpha);
+
+    % model parameters
+    params.dim_state = 5*params.space_dim + params.Nanchor*params.space_dim + params.dim_Gamma;    % done on the observer model (easier to compare)
+
+    % shared position (hyb and EKF)
+    params.pos_p = [1 6 11];   % see mode_rover.m
+    params.pos_v = [2 7 12];   % see mode_rover.m  
+    params.pos_acc = [3 8 13];
+    % positions for hyb
+    params.pos_jerk = [4 9 14];
+    % positions for biases
+    params.pos_bias = [5 10 15];   % IMU bias
+    % rest of stuff
+    params.pos_anchor = [5*params.space_dim+1:params.dim_state-params.dim_Gamma];    % after all the double integrators come the anchors   
+    params.pos_Gamma = [params.pos_anchor(end)+1:params.dim_state];
+    params.pos_fc = [params.pos_p params.pos_v];
+    params.dim_state_est = numel(params.pos_fc);
+
+    % input dim
+    params.dim_input = params.space_dim;   % input on each dimension
+
+    % output dim
+    % distances + accelerations + velocity (only for learning) + position
+    % (only for learning)
+    params.OutDim = params.Nanchor + 3*params.space_dim;  
+    params.observed_state = [];   % not reading the state    
+    params.pos_dist_out = 1:params.Nanchor;
+    params.pos_acc_out = [params.Nanchor + 2*params.space_dim + 1:params.OutDim];
+    params.pos_v_out = [params.Nanchor + params.space_dim + 1:params.Nanchor + 2*params.space_dim];
+    params.pos_p_out = [params.Nanchor + 1:params.Nanchor + params.space_dim];
+    params.OutDim_compare = [params.pos_p_out params.pos_v_out];   % distances
+    
+    % sampling
+    params.IMU_samp = 1;
+    params.UWB_samp = 20;
+    params.UWB_pos = []; 
+
+    % memory
+    params.last_noise = zeros(params.Ntraj,params.OutDim);
+    params.last_D = zeros(params.Ntraj,params.Nanchor);
+    params.last_D_ref = zeros(params.Ntraj,params.Nanchor);
+    params.last_IMU_acc = zeros(params.Ntraj,numel(params.pos_acc_out));
+    params.last_IMU_acc_ref = zeros(params.Ntraj,numel(params.pos_acc_out));
+
+    % derivative of the pjump
+    params.wlen = 4;
+    params.buflen = 10;
+    params.dim_pjump = params.space_dim;
+    for traj = 1:params.Ntraj
+        params.p_jump_time = [];
+        params.p_jump(traj).val = [];
+        params.p_jump_der(traj).val = [];
+        params.p_jump_der_buffer(traj).val = zeros(params.dim_pjump,params.buflen);
+        params.p_jump_der_counter(traj).val = 0;
+    end    
+    
+
+    % noise (on distances + acceleration)
+    params.noise_mat = 0*ones(params.OutDim,2);    
+    % sigma
+    params.noise_mat_original(params.pos_acc_out,1) = 1*3e-1;   % noise on IMU - sigma
+    params.noise_mat_original(params.pos_dist_out,1) = 1*2e-1;  % noise on UWB - sigma    
+    params.mean = params.noise_mat_original(:,1);
+    params.noise_mat(:,1) = 0*params.noise_mat_original(:,1);    
+
+    %%% process noise %%%
+    params.jerk_enable = 0;
+
+    %%%%%% EKF %%%%%
+    % enable noise
+    params.EKF = 0;        
+    params.hyb = 1;
+    params.dryrun = 0;
+
+    %%% noise matrices
+    % measurement noise
+    params.R = diag([params.noise_mat_original(params.pos_dist_out,1).^2.*ones(params.Nanchor,1);     ...  % UWB         
+                     zeros(numel([params.pos_p params.pos_v]),1);                           ...  % P,V
+                     params.noise_mat_original(params.pos_acc_out,1).^2.*ones(params.space_dim,1);    ... % IMU ACC                     
+        ]);      
+    
+    % process noise - model
+    params.Q = 1e0*diag([1e0 1e0 1e0 1e0 1e0 1e0,... % JERK                     
+        ]);
+
+    % EKF covariance matrix
+    for traj=1:params.Ntraj
+        params.Phat(traj).val(1,:,:) = 1e0*eye(params.dim_state);
+    end
+    %%%%%%%%%%%%%%%%
+
+    %%%%%% GENERAL OBS %%%%%
+    % observer stuff
+    params.time_J = [];
+    params.d_true = zeros(params.Nanchor,1);
+    params.d_noise = zeros(params.Nanchor,1);
+    params.d_est = zeros(params.Nanchor,1);
+    %%%%%%%%%%%%%%%%%%%%%%%%
+    
+    % initial condition    
+    params.X(1).val(:,1) = 1*[2;0;0;0;0.1; ...                % x pos + IMU bias
+                              2;0;0;0;0.2; ...                % y pos + IMU bias
+                              0;0;0;0;0.1; ...                % z pos + IMU bias
+                              -an_dp;-an_dp;an_dz;  ...
+                              -an_dp;an_dp;an_dz;   ...
+                              an_dp;an_dp;an_dz;    ...
+                              an_dp;-an_dp;an_dz;   ...    % anchors                                                                          
+                              params.C'; ...              % params                              
+                              params.theta'; ...
+                              params.beta'; ...
+                              params.alpha'];  
+
+    % hills on z - correct initialization
+    p_now = params.X(1).val(params.pos_p(1:2),1);
+    p_est = zeros(1,2);
+    p_grid = [params.X_gauss(1,:); params.Y_gauss(:,1)'];
+    for i=1:2
+        pdiff = p_grid(i,:)-p_now(i);   
+        p_est(i) = find(abs(pdiff) == min(abs(pdiff)),1,'first');                
+    end
+    z0 = params.G_gauss(p_est(1),p_est(2));
+    params.X(1).val(params.pos_p(3),1) = z0; 
+    
+    % position in the state vector of the estimated parameters
+    params.estimated_params = params.pos_Gamma;
+    
+    % which vars am I optimising
+    params.opt_vars = [params.pos_Gamma];
+%     params.opt_vars = [params.pos_p params.pos_v params.pos_acc];
+    
+    % set the not optimised vars
+    tmp = 1:length(params.X(1).val(:,1));
+    tmp_idx = tmp;
+    for i=1:length(params.opt_vars)
+        tmp_idx = intersect(tmp_idx,find(tmp~=params.opt_vars(i)));
+    end
+    params.nonopt_vars = tmp_idx;
+
+    % multistart
+    if params.multistart
+        Npoints = 3;
+        decades = linspace(0,4,Npoints);
+    
+        for i=1:Npoints 
+            params.MULTISTART(i,:) = repmat(decades(i),1,length(params.opt_vars));   
+            params.MULTISTART(i,end-1:end) = params.X(1).val(params.opt_vars(end-1:end),1);
+        end 
+        params.tpoints = CustomStartPointSet(params.MULTISTART(:,1:end));
+        params.X(1).val(params.opt_vars(1:end),1) = params.MULTISTART(1,1:end);
+    end
+
+
+    % same initial condition for all the trajectories (under development)
+    params.multi_traj_var = [params.pos_p]; 
+    pos_init = [3 3;  ...
+                -3 3; ...
+                -3 -3; ...
+                3 -3];
+    for traj=2:params.Ntraj
+        params.X(traj).val(:,1) = params.X(1).val(:,1);
+
+        % random
+        params.X(traj).val(params.multi_traj_var,1) = params.X(1).val(params.multi_traj_var,1).*(1 + 0*5e-1*randn(length(params.multi_traj_var),1));
+
+        % from starting positions
+%         params.X(traj).val(params.pos_p,1) = pos_init(traj,:);
+    end    
+    
+    % plot vars (used to plot the state estimation. When the parameters are
+    % too many, consider to use only the true state components)
+    params.plot_vars = [params.pos_p params.pos_v];
+    params.plot_params = [3 7];
+    params.dim_out_plot = [params.pos_p_out params.pos_v_out];       
+
+    % fminunc
+    params.dist_optoptions = optimoptions('fminunc', 'MaxIter', 1, 'display','off');
+
+    
+end
diff --git a/Lib/models/rover/params_update_rover.m b/Lib/models/rover/params_update_rover.m
new file mode 100644
index 0000000..30a1729
--- /dev/null
+++ b/Lib/models/rover/params_update_rover.m
@@ -0,0 +1,33 @@
+%% PARAMS_UPDATE_ROVER
+% file: params_update_rover.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function updates the estimated parameters on a rover
+% INPUT:
+% params: structure with all the necessary parameters 
+% x: state vector
+% OUTPUT:
+% params_out: updated structure with the new model parameters
+function params_out = params_update_rover(params,x)
+
+    % assign params
+    params_out = params; 
+
+    if ~params.EKF
+
+        params_out.theta(1:5) = x(params.pos_Gamma(3:7));
+%         params_out.alpha(1:end) = x(params.pos_Gamma(10:end));  
+
+        % normal beta
+        params_out.beta(1) = x(params.pos_Gamma(8));
+        params_out.beta(2) = x(params.pos_Gamma(9));
+
+        % passband
+        params_out.C(1:2) = x(params.pos_Gamma(1:2));
+%         params_out.beta(1) = 1;
+%         params_out.beta(2) = -params_out.C(2);
+%         params_out.beta = params_out.theta(2)*params_out.beta;
+    end
+        
+    
+end
\ No newline at end of file
diff --git a/Lib/obs/EKF/EKF_armesto.m b/Lib/obs/EKF/EKF_armesto.m
new file mode 100644
index 0000000..5096daf
--- /dev/null
+++ b/Lib/obs/EKF/EKF_armesto.m
@@ -0,0 +1,128 @@
+%% EKF.m
+% file: ekf.m
+% author: Federico Oliva
+% date: 30/01/2023
+% description: EKF observer
+% INPUT: obs, state, measure
+% OUTPUT: obs
+
+function obs = EKF(obs,xhat_kk_1,y_k)
+
+    % get params
+    params = obs.init.params;
+
+    % get time instant
+    k = obs.init.ActualTimeIndex;
+    tspan = obs.setup.time(max(1,k-1):k);
+
+    % get traj
+    traj = obs.init.traj;    
+
+    % get control
+    uhat_kk_1 = obs.init.input_story_ref(traj).val(:,k-1);
+
+    % get initial covariance
+    sz = size(params.Phat(traj).val);
+    Phat_0 = reshape(params.Phat(traj).val(k-1,:,:),sz(2),sz(3));  
+
+    % prediction step (eq. 11)  NB: check if the correct input is used ins.
+    X = obs.setup.ode(@(t,x)obs.setup.model(t, x, obs.setup.params, obs), tspan, xhat_kk_1,params.odeset); 
+    xhat_k = X.y(:,end);
+
+    % predicted measure (and store in obs)
+    yhat_k = obs.setup.measure(xhat_k,params,tspan(1),uhat_kk_1,obs);
+    obs.init.Yhat_full_story(traj).val(1,:,k) = yhat_k;
+
+    % init downsampling
+    posH_row = [];    
+    yhat_ks = [];
+    y_ks = [];
+
+    % get downsampling - CAM
+    if mod(k,params.CAM_samp) == 0
+        posH_row = [posH_row, [params.pos_p_out params.pos_quat_out]];
+        yhat_ks = [yhat_ks; yhat_k([params.pos_p_out params.pos_quat_out])];
+        y_ks = [y_ks; y_k([params.pos_p_out params.pos_quat_out])];        
+    end
+
+    % get downsampling - IMU
+    if mod(k,params.IMU_samp) == 0
+        posH_row = [posH_row, [params.pos_acc_out params.pos_omega_out]];
+        yhat_ks = [yhat_ks; yhat_k([params.pos_acc_out params.pos_omega_out])];
+        y_ks = [y_ks; y_k([params.pos_acc_out params.pos_omega_out])];
+    end
+
+    % get jacobians    
+    [GFx, GFw, GHx] = G(xhat_kk_1,params.Ts,y_k);      
+    GHs = GHx(posH_row,:);
+    
+    % project covariance (eq. 12)
+    Phat_kk_1 = GFx*Phat_0*GFx' + GFw*params.Q*GFw';    
+
+    % update
+    if ~isempty(posH_row)
+
+        % correction term (eq. 13)        
+        Ks = (Phat_kk_1*GHs')*(pinv(GHs*Phat_kk_1*GHs' + params.R(posH_row,posH_row)));        
+
+        % correction step (eq. 14-15)
+        xnew = xhat_k + Ks*(y_ks-yhat_ks);
+        Pnew = Phat_kk_1 - Ks*GHs*Phat_kk_1;
+    else
+        xnew = xhat_k;
+        Pnew = Phat_kk_1;
+    end
+
+    %%% TEST %%%    
+    xnew(params.pos_bias) = xnew(params.pos_bias);
+
+    % update
+    obs.init.X_est(traj).val(:,k) = xnew;
+    obs.init.params.Phat(traj).val(k,:,:) = Pnew;
+
+end
+
+%%%%
+% Jacobian of df/d(x,w) and dh/dx
+function [GFx, GFw, GHx] = G(x,T,y)    
+
+    %%% for the time being the centripetal mode is not included so there is
+    %%% no need to add the derivatives of the mappings wrt omega, quat,
+    %%% alpha.
+
+    % df/dx
+    %      P            V          ACC                B             Q             W             J           ALPHA      B'    
+    GFx = [eye(3)       T*eye(3)   0.5*T^2*eye(3)     zeros(3)      zeros(3,4)    zeros(3)      zeros(3)    zeros(3)   zeros(3);   ... P
+           zeros(3)     eye(3)     T*eye(3)           zeros(3)      zeros(3,4)    zeros(3)      zeros(3)    zeros(3)   zeros(3);   ... V
+           zeros(3)     zeros(3)   eye(3)             zeros(3)      zeros(3,4)    zeros(3)      zeros(3)    zeros(3)   zeros(3);   ... ACC
+           zeros(3)     zeros(3)   zeros(3)           1*eye(3)        zeros(3,4)    zeros(3)      zeros(3)    zeros(3)   zeros(3); ... B
+           zeros(4,3)   zeros(4,3) zeros(4,3)         zeros(4,3)    zeros(4)      zeros(4,3)    zeros(4,3)  zeros(4,3) zeros(4,3); ... Q
+           zeros(3)     zeros(3)   zeros(3)           zeros(3)      zeros(3,4)    zeros(3)      zeros(3)    zeros(3)   zeros(3);   ... OMEGA
+           zeros(3)     zeros(3)   zeros(3)           zeros(3)      zeros(3,4)    zeros(3)      zeros(3)    zeros(3)   zeros(3);   ... JERK
+           zeros(3)     zeros(3)   zeros(3)           zeros(3)      zeros(3,4)    zeros(3)      zeros(3)    zeros(3)   zeros(3);   ... ALPHA
+           zeros(3)     zeros(3)   zeros(3)           zeros(3)      zeros(3,4)    zeros(3)      zeros(3)    zeros(3)   zeros(3);   ... B'
+           ];
+
+    % df/dw    
+    %      J               ALPHA        B'  
+    GFw = [T^3/6*eye(3)    zeros(3)     zeros(3);   ... P
+           0.5*T^2*eye(3)  zeros(3)     zeros(3);   ... V
+           T*eye(3)        zeros(3)     zeros(3);   ... ACC
+           zeros(3)        zeros(3)     T*eye(3);   ... B
+           zeros(4,3)      zeros(4,3)   zeros(4,3); ... Q
+           zeros(3)        zeros(3)     zeros(3);   ... OMEGA
+           zeros(3)        zeros(3)     zeros(3);   ... JERK
+           zeros(3)        zeros(3)     zeros(3);   ... ALPHA
+           zeros(3)        zeros(3)     zeros(3);   ... B'
+           ];
+
+    % dh/dx
+    %      P            V          ACC                B             Q             W             J           ALPHA      B'    
+    GHx = [eye(3)       zeros(3)   zeros(3)           1*eye(3)      zeros(3,4)    zeros(3)      zeros(3)    zeros(3)   zeros(3);    ... P
+           zeros(4,3)   zeros(4,3) zeros(4,3)         zeros(4,3)    eye(4)        zeros(4,3)    zeros(4,3)  zeros(4,3) zeros(4,3);  ... Q
+           zeros(3)     zeros(3)   eye(3)             0*eye(3)      zeros(3,4)    zeros(3)      zeros(3)    zeros(3)   zeros(3);    ... ACC
+           zeros(3)     zeros(3)   zeros(3)           zeros(3)      zeros(3,4)    eye(3)        zeros(3)    zeros(3)   zeros(3);    ... OMEGA
+        ];
+
+
+end
\ No newline at end of file
diff --git a/Lib/obs/EKF/EKF_rover.m b/Lib/obs/EKF/EKF_rover.m
new file mode 100644
index 0000000..7b3e8a1
--- /dev/null
+++ b/Lib/obs/EKF/EKF_rover.m
@@ -0,0 +1,330 @@
+%% EKF.m
+% file: ekf.m
+% author: Federico Oliva
+% date: 30/01/2023
+% description: EKF observer
+% INPUT: obs, state, measure
+% OUTPUT: obs
+
+function [obs, xnew] = EKF_rover(obs,xhat_kk_1,y_k)    
+
+    % get params
+    params = obs.init.params;
+    P = params;
+
+    % short state
+    Xpos = [P.pos_p, P.pos_v, P.pos_bias, P.pos_acc, P.pos_quat, P.pos_w, P.pos_bias_w];
+
+    % get time instant
+    k = obs.init.ActualTimeIndex;
+    tspan = obs.setup.time(max(1,k-1):k);
+
+    % get traj
+    traj = obs.init.traj;    
+
+    % get control
+    uhat_kk_1 = obs.init.input_story_ref(traj).val(:,max(1,k-1));
+
+    % get initial covariance
+    sz = size(P.Phat(traj).val);
+    if mod(k,P.PhatReset) == 0
+        P.Phat(traj).val(max(1,1),:,:) = P.PhatZero;
+    end
+    Phat_0 = reshape(P.Phat(traj).val(max(1,1),:,:),sz(2),sz(3));  
+    Phat_0 = Phat_0(Xpos,Xpos);
+
+    % prediction step (eq. 11)  NB: check if the correct input is used ins.        
+    try
+        X = obs.setup.ode(@(t,x)obs.setup.model(t, x, P, obs), tspan, xhat_kk_1, P.odeset); 
+    catch
+        warning('EKF: 1 element in time span. Setting integration to initial condition.')
+        X.y(:,1) = xhat_kk_1;
+    end
+    xhat_k = X.y(:,end);        
+
+    % predicted measure (and store in obs)
+    yhat_k = obs.setup.measure(xhat_k,P,tspan,uhat_kk_1,obs);
+    obs.init.Yhat_full_story(traj).val(1,:,k) = yhat_k;
+
+    % init downsampling
+    posH_row = [];    
+    yhat_ks = [];
+    y_ks = [];
+
+    % get downsampling - UWB
+    if mod(k,P.UWB_samp) == 0
+        posH_row = [posH_row, [P.pos_dist_out]];
+        yhat_ks = [yhat_ks; yhat_k([P.pos_dist_out])];
+        y_ks = [y_ks; y_k([P.pos_dist_out])];        
+    end
+
+    % get downsampling - IMU
+    if mod(k,P.IMU_samp) == 0
+        posH_row = [posH_row, [P.pos_acc_out]];
+        yhat_ks = [yhat_ks; yhat_k([P.pos_acc_out])];
+        y_ks = [y_ks; y_k([P.pos_acc_out])];
+    end
+
+    % get downsampling - GYRO
+    if mod(k,P.Gyro_samp) == 0
+        posH_row = [posH_row, [P.pos_w_out]];
+        yhat_ks = [yhat_ks; yhat_k([P.pos_w_out])];
+        y_ks = [y_ks; y_k([P.pos_w_out])];
+    end
+
+    % get jacobians    
+    [GFx, GFw, GHx] = G(xhat_kk_1,P.Ts,y_k,P);      
+    GHs = GHx(posH_row,:);
+
+    % test sensitivity
+    if mod(k,P.UWB_samp) == 0
+        obs.init.GHx(k,:,:) = GHx; 
+    end
+
+    O = obsv(GFx,GHs);
+    obs.init.OBS(k) = rank(O);
+        
+
+    % resize
+    xhat_kk_1 = xhat_kk_1(Xpos);
+    xhat_k = xhat_k(Xpos);
+    
+    % project covariance (eq. 12)
+    Phat_kk_1 = GFx*Phat_0*GFx' + GFw*P.Q*GFw';    
+
+    % update
+    if ~isempty(posH_row)
+
+        % correction term (eq. 13)        
+        Ks = 1*(Phat_kk_1*GHs')*(pinv(GHs*Phat_kk_1*GHs' + P.R(posH_row,posH_row)));        
+
+        % correction step (eq. 14-15)
+        mismatch = (y_ks-yhat_ks);
+        correction = Ks*mismatch;
+
+        xnew = xhat_k + 1*correction;
+        xnew(P.pos_quat) = quatnormalize(xnew(P.pos_quat)');
+        Pnew = Phat_kk_1 - Ks*GHs*Phat_kk_1;
+    else
+        xnew = xhat_k;
+        Pnew = Phat_kk_1;
+        correction = zeros(numel(xnew(P.pos_trasl)),1);
+    end    
+
+    % lerciata
+    xold = xnew;
+    if xnew(3) >= P.an_dz
+        dz = xnew(3) - P.an_dz;
+        xnew(3) = max(0,xnew(3) - 2*dz);
+    end
+
+    % update
+    obs.init.X_est(traj).val(Xpos,k) = xnew;
+    obs.init.params.Phat(traj).val(1,Xpos,Xpos) = Pnew;
+    obs.init.params.correction_story(:,k) = correction;
+
+    % return a 55 array    
+    xnew = obs.init.X_est(traj).val(:,k);       
+
+end
+
+%%%%
+% Jacobian of df/d(x,w) and dh/dx
+function [GFx, GFw, GHx] = G(x,T,y,P)   
+
+    % quaternion
+    q = x(P.pos_quat);
+
+    % position
+    p = x(P.pos_p);
+
+    % angula velocity
+    w = x(P.pos_w);
+
+    % tag position
+    delta = P.TagPos;
+
+    % Rotation matrix derivative
+    [M1, M2, M3, M4] = M(q);
+
+    %%% df/dx %%%
+    GFx = zeros(P.dim_state);
+    % dp/dv
+    GFx(P.pos_p,P.pos_v) = eye(P.space_dim);
+    % dv/da
+    GFx(P.pos_v,P.pos_acc) = eye(P.space_dim);
+    % dq/dq
+    O = OMEGA(w);
+    GFx(P.pos_quat,P.pos_quat) = 0.5*O;
+    % dq/dw
+    [S1, S2, S3] = S();
+    GFx(P.pos_quat,P.pos_w) = 0.5*[S1*q S2*q S3*q];
+
+    % to discrete
+    Xpos = [P.pos_p, P.pos_v, P.pos_bias, P.pos_acc, P.pos_quat, P.pos_w, P.pos_bias_w];
+    Xpospos = [P.pos_p, P.pos_v, P.pos_bias, P.pos_acc];
+    Xposquat = [P.pos_quat, P.pos_w, P.pos_bias_w];
+    % GFx(Xpos,Xpos) = eye(numel(Xpos)) + GFx(Xpos,Xpos)*T;
+    GFx(Xpospos,Xpospos) = eye(numel(Xpospos)) + GFx(Xpospos,Xpospos)*T;
+    GFx(Xposquat,Xposquat) = eye(numel(Xposquat)) + GFx(Xposquat,Xposquat)*T;
+
+    %%% df/dw %%%
+    Wpos_bias = 1:3;
+    Wpos_acc = 4:6;
+    Wpos_w = 7:9;
+    Wpos = [Wpos_bias Wpos_acc Wpos_w];
+    Wpospos = [Wpos_bias Wpos_acc];
+    Wposquat = [Wpos_w];
+    GFw = zeros(P.dim_state,numel(Wpos));
+
+    % df/dw
+    GFw(P.pos_bias,Wpos_bias) = eye(3);
+    GFw(P.pos_acc,Wpos_acc) = eye(3);
+    GFw(P.pos_w,Wpos_w) = eye(3);
+
+    %%% dh/dx %%%    
+    GHx = zeros(P.OutDim,P.dim_state);
+    % dyd/dx
+    % cycle over the tags and anchors
+    for i=1:P.Ntags
+        for j=1:P.Nanchor
+
+            % Lij
+            Lij = L(x,i,j,P);
+
+            % MU
+            [MUij_x, MUij_y, MUij_z] = MU(x,i,j,P);
+
+            % dydij/dp
+            GHx(P.pos_dist_out(P.Nanchor*(i-1)+j),P.pos_p) = 1/Lij*[MUij_x, MUij_y, MUij_z];
+
+            % dydij/dq
+            GHx(P.pos_dist_out(P.Nanchor*(i-1)+j),P.pos_quat(1)) = 1/Lij*[MUij_x, MUij_y, MUij_z]*M1*delta(:,i);
+            GHx(P.pos_dist_out(P.Nanchor*(i-1)+j),P.pos_quat(2)) = 1/Lij*[MUij_x, MUij_y, MUij_z]*M2*delta(:,i);
+            GHx(P.pos_dist_out(P.Nanchor*(i-1)+j),P.pos_quat(3)) = 1/Lij*[MUij_x, MUij_y, MUij_z]*M3*delta(:,i);
+            GHx(P.pos_dist_out(P.Nanchor*(i-1)+j),P.pos_quat(4)) = 1/Lij*[MUij_x, MUij_y, MUij_z]*M4*delta(:,i);
+
+        end
+    end
+    % dyw/dw
+    GHx(P.pos_w_out,P.pos_w) = eye(3);
+    % dya/dba
+    GHx(P.pos_acc_out,P.pos_bias) = eye(3);
+    % dya/dbacc
+    GHx(P.pos_acc_out,P.pos_acc) = eye(3);
+
+    % resize
+    GFx = GFx(Xpos,Xpos);
+    GFw = GFw(Xpos,:);
+    GHx = GHx(:,Xpos);
+
+end
+
+%%% get the Omega matrix
+function [S1, S2, S3] = S()
+
+    S1 = [   0    0    0    1;
+             0    0   -1    0;
+             0    1    0    0;
+            -1    0    0    0
+         ];
+
+    S2 = [   0    0    1    0;
+             0    0    0    1;
+            -1    0    0    0;
+             0   -1    0    0
+         ];
+
+    S3 = [   0   -1    0    0;
+             1    0    0    0;
+             0    0    0    1;
+             0    0   -1    0
+         ];
+end
+
+%%% get the Omega matrix
+function O = OMEGA(W)
+    S = [0      -W(3)   +W(2); ...
+         +W(3)  0       -W(1); ...
+         -W(2)  +W(1)   0];
+    O = [+S     W; ...
+         -W'    0];
+end
+
+%%% get the L norm: i= Tag j = Anchor
+function LOUT = L(x,i,j,P)
+
+    [MUx, MUy, MUz] = MU(x,i,j,P);
+
+    % L
+    LOUT = norm([MUx, MUy, MUz]);
+
+end
+
+%%% get the Mu vector: i= Tag j = Anchor
+function [MUx, MUy, MUz] = MU(x,i,j,P)
+
+    % get position
+    p = x(P.pos_p);
+
+    % get quaternion
+    q = x(P.pos_quat);
+    R = ROT(q);
+
+    % delta
+    delta = P.TagPos(:,i);
+
+    % anchor
+    A = x(P.pos_anchor(3*(j-1)+[1:3]));
+
+    % MU
+    MUx = p(1) + R(1,:)*delta - A(1);
+    MUy = p(2) + R(2,:)*delta - A(2);
+    MUz = p(3) + R(3,:)*delta - A(3);
+
+end
+
+%%% get the Rotation matrix derivative
+function [M1, M2, M3, M4] = M(q)
+
+    M1 = 2*[  +q(1)    +q(4)    -q(3);
+              -q(4)    +q(1)    +q(2);
+              +q(3)    -q(2)    +q(1);
+           ];
+
+    M2 = 2*[  +q(2)    +q(3)    +q(4);
+              +q(3)    -q(2)    +q(1);
+              +q(4)    -q(1)    -q(2);
+           ];
+
+    M3 = 2*[  -q(3)    +q(2)    -q(1);
+              +q(2)    +q(3)    +q(4);
+              +q(1)    +q(4)    -q(3);
+           ];
+
+    M4 = 2*[  -q(4)    +q(1)    +q(2);
+              -q(1)    -q(4)    +q(3);
+              +q(2)    +q(3)    +q(4);
+           ];
+
+end
+
+%%% get the Rotation matrix
+function R = ROT(q)
+
+    % column 1
+    R(1,1) = q(2)^2 - q(3)^2 - q(4)^2 + q(1)^2;
+    R(2,1) = 2*(q(2)*q(3) - q(4)*q(1));
+    R(3,1) = 2*(q(2)*q(4) + q(3)*q(1));
+
+    % column 2
+    R(1,2) = 2*(q(2)*q(3) + q(4)*q(1));
+    R(2,2) = -q(2)^2 + q(3)^2 - q(4)^2 + q(1)^2;
+    R(3,2) = 2*(q(3)*q(4) - q(2)*q(1));
+
+    % column 3
+    R(1,3) = 2*(q(2)*q(4) - q(3)*q(1));
+    R(2,3) = 2*(q(3)*q(4) + q(2)*q(1));
+    R(3,3) = -q(2)^2 - q(3)^2 + q(4)^2 + q(1)^2;
+
+end
\ No newline at end of file
diff --git a/Lib/obs/obsopt.m b/Lib/obs/obsopt.m
index 7b75353..7ca468f 100644
--- a/Lib/obs/obsopt.m
+++ b/Lib/obs/obsopt.m
@@ -69,6 +69,7 @@
                 
                 % output dimension
                 obj.setup.dim_out = params.OutDim;
+                obj.setup.dim_out_compare = params.OutDim_compare;
             
                 % state dimension
                 obj.setup.dim_state = params.StateDim;
@@ -90,8 +91,11 @@
                 obj.setup.opt_vars = params.opt_vars;
                 obj.setup.nonopt_vars = params.nonopt_vars;
                 obj.setup.plot_vars = params.plot_vars;
+                obj.setup.plot_params = params.plot_params;
                                 
             else
+
+                % default dynamics
                 obj.setup.model = @(t,x,params) -2*x;
                 obj.setup.measure = @(x,params) x;
                 obj.setup.ode = @ode45;
@@ -101,6 +105,7 @@
                 % the sample time previously set.
                 obj.setup.time = 0:obj.setup.Ts:10;
                 
+                % default output
                 obj.setup.dim_out = 1;
                 obj.setup.dim_state = 1;
                 
@@ -170,38 +175,127 @@
             else
                 obj.setup.AdaptiveSampling = 0;
             end
+
+            % dafault wavelet setup. Check CWT method for info on the
+            % parameters
+            obj.init.nfreqs = 2;            
+            obj.init.freqs = zeros(obj.init.nfreqs,1);
+            obj.init.wvname = 'amor';
+            obj.init.Nv = 48;
+            obj.init.PLIMITS = [1e0*obj.setup.Ts 2e2*obj.setup.Ts];
+            obj.init.FLIMITS = fliplr(1./obj.init.PLIMITS);            
+            obj.init.NtsChanged = 0;
+            obj.init.break = 0;
+                        
+            
+            % option to define the safety interval in adaptive sampling. If
+            % AdaptiveSampling flag is zero the histeresis is set to zero,
+            % disabling the adaptive sampling
+            if any(strcmp(varargin,'AdaptiveParams')) && (obj.setup.AdaptiveSampling)
+                pos = find(strcmp(varargin,'AdaptiveParams'));
+                tmp = varargin{pos+1};    
+
+                % The Adaptive sampling can work both with the signal
+                % richness metric eq. (3.23), and with the wavelet
+                % transform paragraph (3.53). Now we detail the wavelet
+                % parameters
+
+                % Nyquist frequency. Integer defining how far from the
+                % estimated bandwith we will consider frequencies in the
+                % wavelet
+                obj.init.Fnyq = tmp(1);     
+
+                % Length of the buffer considered for the wavelet
+                % transform. It's the N_{\Psi} parameter in eq. (3.32)
+                obj.init.Fbuflen = tmp(2);  % integer < Nw
+
+                % CWT will return a range of frequencies. You can decide to
+                % choose the minimum (1) or the maximum (2) as your
+                % bandwidth estimation. Clearly, max is the theoretical
+                % better choice, so it is the default. Min was left there
+                % after testing and legacy. Also, as it is multiplied by
+                % Fnyq, a proper parameter tweak can avoid messing up with
+                % the setup. Fselect is Fmax in eq. (3.32)
+                obj.init.Fselect = tmp(3);  % 1 = min 2 = max 
+
+                % Fsamp is computed in eq. (3.33a) with Fselect and Fnyq
+
+                % The wavelet buffer is considered equally sampled by FNts.
+                % The default (and best) choiche would be FNts=1, so to
+                % catch all the frequencial info in the signal. However,
+                % for specific cases, you could want to downsample the
+                % measurement a bit. For instance when there are different
+                % frequency scales to be considered. FNts is to be replaced
+                % with the 1 in eq. (3.33b), the first argument of the max
+                % computation. 
+                obj.init.FNts = tmp(4);
+
+                % What is the minimum frequency considered as an acceptable
+                % CWT output? CWT could return a meaningless Fmax=1e-5, so
+                % it is considered only if it is > Fmin
+                obj.init.Fmin = tmp(5);
+
+                % Adaptive sampling is considered only if the estimation
+                % error is over a certain threshold, which is this epsD, (epsilon Delta)
+                % Refer to eq. (3.23)
+                obj.init.epsD = tmp(6);
+
+                % Here you can decide either to use the signal richness
+                % Adaptive sampling (PE_flag = 1) or the wavelet one (PE_flag = 0)
+                obj.init.PE_flag = tmp(7);
+
+                % On which elements of the output measurements do you want
+                % to compute the wavelet? Currently, reliable tests have
+                % been done only on scalar output.
+                obj.init.wavelet_output_dim = tmp(8:end);
+            else
+
+                % Default Adaptive sampling parameters
+                obj.init.FNts = 1;
+                obj.init.Fbuflen = 20;
+                obj.init.Fselect = 2;
+                obj.init.Fnyq = 2;
+                obj.init.FNts = 0;
+                obj.init.PE_flag = 1;
+                obj.init.epsD = Inf;
+                obj.init.wavelet_output_dim = [];
+            end
             
             % enable or not the buffer flush on the adaptive sampling
-            if any(strcmp(varargin,'FlushBuffer'))
+            if any(strcmp(varargin,'FlushBuffer')) && (obj.setup.AdaptiveSampling)
                 pos = find(strcmp(varargin,'FlushBuffer'));
                 obj.setup.FlushBuffer = (obj.setup.AdaptiveSampling && varargin{pos+1});
             else
                 obj.setup.FlushBuffer = 0;
             end
             
-            % option to define the safety interval in adaptive sampling. If
-            % AdaptiveSampling flag is zero the histeresis is set to zero,
-            % disabling the adaptive sampling
-            if any(strcmp(varargin,'AdaptiveHist')) && (obj.setup.AdaptiveSampling)
-                pos = find(strcmp(varargin,'AdaptiveHist'));
-                tmp = varargin{pos+1};
-                obj.setup.dJ_low = tmp(1);
-                obj.setup.dJ_high = tmp(2);
-                obj.setup.dPE = tmp(3);
-            else
-                obj.setup.dJ_low = 0;
-                obj.setup.dJ_high = 0;
-                obj.setup.dPE = -1;
-            end
-            
             % get the maximum number of iterations in the optimisation
             % process. Default is 100.
             if any(strcmp(varargin,'MaxIter'))
                 pos = find(strcmp(varargin,'MaxIter'));
-                obj.setup.max_iter = varargin{pos+1};
+                obj.setup.max_iterVal = varargin{pos+1};
+                if length(obj.setup.max_iterVal) == 1
+                    obj.setup.max_iter = obj.setup.max_iterVal;                    
+                else
+                    obj.setup.max_iter = max(obj.setup.max_iterVal);
+                end
             else
                 obj.setup.max_iter = 100;
             end
+            obj.init.MaxIter_story = [];
+            
+            % get the maximum number of iterations in the optimisation
+            % process. Default is 100.
+            if any(strcmp(varargin,'MaxOptTime'))
+                pos = find(strcmp(varargin,'MaxOptTime'));
+                obj.setup.MaxOptTime = varargin{pos+1};
+                obj.setup.MaxOptTimeFlag = 0;
+            else
+                obj.setup.MaxOptTime = Inf;
+                obj.setup.MaxOptTimeFlag = 0;
+            end
+            % init story
+            obj.init.MaxOptTime_story = [];
             
             % check if the optimised value shall always be accepted. This
             % goes in contrast with the role of Jdot_thresh
@@ -225,10 +319,17 @@
             % more detailed information. Default is 3.
             if any(strcmp(varargin,'Nts'))
                 pos = find(strcmp(varargin,'Nts'));
-                obj.setup.Nts = varargin{pos+1};
+                obj.setup.NtsVal = varargin{pos+1};
+                if length(obj.setup.NtsVal) == 1
+                    obj.setup.Nts = obj.setup.NtsVal;
+                    obj.setup.NtsVal = ones(1,obj.setup.w)*obj.setup.Nts;
+                else
+                    obj.setup.Nts = min(obj.setup.NtsVal);
+                end
             else
                 obj.setup.Nts = 3;
             end
+            obj.init.Nsaved = 0;
             
             % Discrete sampling time (for filters)
             obj.setup.DTs = obj.setup.Nts*obj.setup.Ts;
@@ -253,6 +354,14 @@
                 obj.setup.J_thresh = [1e-10, 1e3];
             end
             
+            % normalise the cost function
+            if any(strcmp(varargin,'J_normalise'))
+                pos = find(strcmp(varargin,'J_normalise'));
+                obj.setup.J_normalise = varargin{pos+1};
+            else
+                obj.setup.J_normalise = 1;
+            end
+            
             % change max iterations depending on PE 
             if any(strcmp(varargin,'PE_maxiter'))
                 pos = find(strcmp(varargin,'PE_maxiter'));
@@ -260,6 +369,14 @@
             else
                 obj.setup.PE_maxiter = 0;
             end
+            
+            % PE position
+            if any(strcmp(varargin,'PEPos'))
+                pos = find(strcmp(varargin,'PEPos'));
+                obj.setup.PEPos = varargin{pos+1};
+            else
+                obj.setup.PEPos = [1 1];
+            end
 
             % get the optimisation method. Default is fminsearch from
             % MATLAB
@@ -268,57 +385,125 @@
                 obj.setup.fmin = varargin{pos+1};
             else
                 obj.setup.fmin = @fminsearch;
+            end                        
+            
+            % get the multistart option
+            if any(strcmp(varargin,'MultiStart'))
+                pos = find(strcmp(varargin,'MultiStart'));
+                obj.setup.MultiStart = varargin{pos+1};
+            else
+                obj.setup.MultiStart = 0;
+            end
+            
+            % get the multistart option
+            test = func2str(obj.setup.fmin);
+            if any(strcmp(varargin,'Bounds')) && ~strcmp(test,'fmincon')
+                pos = find(strcmp(varargin,'Bounds'));
+                obj.setup.bounds = varargin{pos+1};                
+            else
+                obj.setup.bounds = 0;
+            end
+            
+            % get the multistart option
+            if any(strcmp(varargin,'BoundsPos'))
+                pos = find(strcmp(varargin,'BoundsPos'));
+                obj.setup.boundsPos = varargin{pos+1};                
+            else
+                obj.setup.boundsPos = [];
+            end            
+            
+            % get the multistart option
+            if any(strcmp(varargin,'BoundsValLow'))
+                pos = find(strcmp(varargin,'BoundsValLow'));
+                obj.setup.boundsValLow = varargin{pos+1};                
+            else
+                obj.setup.boundsValLow = [];
             end
             
+            % get the multistart option
+            if any(strcmp(varargin,'BoundsValUp'))
+                pos = find(strcmp(varargin,'BoundsValUp'));
+                obj.setup.boundsValUp = varargin{pos+1};                
+            else
+                obj.setup.boundsValUp = [];
+            end
+            
+            % get the multistart option
+            if any(strcmp(varargin,'BoundsWeight'))
+                pos = find(strcmp(varargin,'BoundsWeight'));
+                obj.setup.boundsWeight = varargin{pos+1};                
+            else
+                obj.setup.boundsWeight = ones(1,numel(obj.setup.boundsPos));
+            end
+            
+            % constraint pos
+            if any(strcmp(varargin,'ConPos'))
+                pos = find(strcmp(varargin,'ConPos'));
+                obj.setup.ConPos = varargin{pos+1};
+            else
+                obj.setup.ConPos = [];
+            end
+            
+            
             % handle fmincon
             try
                 test = func2str(obj.setup.fmin);
             catch
                 test = 'null';
             end
-            if strcmp(test,'fmincon')
+            if strcmp(test,'fmincon') || strcmp(test,'fminsearchbnd') || any(strcmp(varargin,'Bounds'))
                 if any(strcmp(varargin,'Acon'))
                     pos = find(strcmp(varargin,'Acon'));
                     obj.setup.Acon = varargin{pos+1};
                 else
-                    obj.setup.Acon = [];
+                    obj.setup.Acon = [];                    
                 end
                 if any(strcmp(varargin,'Bcon'))
                     pos = find(strcmp(varargin,'Bcon'));
                     obj.setup.Bcon = varargin{pos+1};
                 else
-                    obj.setup.Bcon = [];
+                    obj.setup.Bcon = [];                
                 end
                 if any(strcmp(varargin,'Acon_eq'))
                     pos = find(strcmp(varargin,'Acon_eq'));
                     obj.setup.Acon_eq = varargin{pos+1};
                 else
-                    obj.setup.Acon_eq = [];
+                    obj.setup.Acon_eq = [];                
                 end
                 if any(strcmp(varargin,'Bcon_eq'))
                     pos = find(strcmp(varargin,'Bcon_eq'));
                     obj.setup.Bcon_eq = varargin{pos+1};
                 else
-                    obj.setup.Bcon_eq = [];
+                    obj.setup.Bcon_eq = [];                
                 end
                 if any(strcmp(varargin,'LBcon'))
+                    obj.setup.LBcon = double(-Inf*ones(1,length(obj.setup.opt_vars)));
                     pos = find(strcmp(varargin,'LBcon'));
-                    obj.setup.LBcon = varargin{pos+1};
+                    obj.setup.LBcon(obj.setup.ConPos) = varargin{pos+1};
                 else
-                    obj.setup.LBcon = [];
+                    obj.setup.LBcon = zeros(1,length(obj.setup.opt_vars));                
                 end
                 if any(strcmp(varargin,'UBcon'))
+                    obj.setup.UBcon = double(Inf*ones(1,length(obj.setup.opt_vars)));
                     pos = find(strcmp(varargin,'UBcon'));
-                    obj.setup.UBcon = varargin{pos+1};
+                    obj.setup.UBcon(obj.setup.ConPos) = varargin{pos+1};
                 else
-                    obj.setup.UBcon = [];
+                    obj.setup.UBcon = zeros(1,length(obj.setup.opt_vars));          
                 end
                 if any(strcmp(varargin,'NONCOLcon'))
                     pos = find(strcmp(varargin,'NONCOLcon'));
-                    obj.setup.NONCOLcon = varargin{pos+1};
+                    obj.setup.NONCOLcon = varargin{pos+1}; 
                 else
-                    obj.setup.NONCOLcon = [];
+		    obj.setup.NONCOLcon = @obj.NONCOLcon;
                 end
+            else
+                obj.setup.Acon = [];    
+                obj.setup.Bcon = [];
+                obj.setup.Acon_eq = [];
+                obj.setup.Bcon_eq = [];
+                obj.setup.LBcon = [];
+                obj.setup.UBcon = [];
+                obj.setup.NONCOLcon = [];
             end
             
             % store J terms
@@ -340,24 +525,32 @@
                 obj.setup.forward = varargin{pos+1};
             else
                 obj.setup.forward = 1;
-            end
+            end            
             
-            % number of reference trajectories (>1 for control design)
-            if any(strcmp(varargin,'control_design'))
-                pos = find(strcmp(varargin,'control_design'));
-                obj.setup.control_design = varargin{pos+1};
+            % wait all buffer to be filled
+            if any(strcmp(varargin,'WaitAllBuffer'))
+                pos = find(strcmp(varargin,'WaitAllBuffer'));
+                obj.setup.WaitAllBuffer = varargin{pos+1};
             else
-                obj.setup.control_design = 0;
+                obj.setup.WaitAllBuffer = 0;
             end
             
             % reference model
-            if any(strcmp(varargin,'model_reference')) && (obj.setup.control_design)
+            if any(strcmp(varargin,'model_reference'))
                 pos = find(strcmp(varargin,'model_reference'));
                 obj.setup.model_reference = varargin{pos+1};
             else
                 obj.setup.model_reference = obj.setup.model;
             end
             
+            % reference measure
+            if any(strcmp(varargin,'measure_reference'))
+                pos = find(strcmp(varargin,'measure_reference'));
+                obj.setup.measure_reference = varargin{pos+1};
+            else
+                obj.setup.measure_reference = obj.setup.measure;
+            end
+            
             
             % get the cost function terms. Default uses measures only, 
             % without any additional filter (e.g. [1 0 0])
@@ -372,18 +565,44 @@
             % filters
             obj.setup.J_temp_scale = temp_scale;
             obj.setup.J_nterm = length(temp_scale);
-            obj.setup.J_nterm_total = length(temp_scale); 
-            
-            % get the spring like term in the cost function
-            if any(strcmp(varargin,'spring'))
-                pos = find(strcmp(varargin,'spring'));
-                obj.setup.J_term_spring = 1;
-                obj.setup.J_temp_scale = [obj.setup.J_temp_scale, varargin{pos+1}];
-                obj.setup.J_term_spring_position = length(obj.setup.J_temp_scale);
-                obj.setup.J_nterm_total = obj.setup.J_nterm_total+1;
+            obj.setup.J_nterm_total = length(temp_scale);             
+            
+            % get the terminal like term in the cost function
+            if any(strcmp(varargin,'terminal'))
+                pos = find(strcmp(varargin,'terminal'));
+                if varargin{pos+1} ~= 0
+                    obj.setup.J_term_terminal = 1;
+                    obj.setup.J_temp_scale = [obj.setup.J_temp_scale, varargin{pos+1}];
+                    obj.setup.J_term_terminal_position = length(obj.setup.J_temp_scale);
+                    obj.setup.J_nterm_total = obj.setup.J_nterm_total+1;
+                else
+                    obj.setup.J_term_terminal = 0;
+                end
             else
-                obj.setup.J_term_spring = 0;
+                obj.setup.J_term_terminal = 0;
+            end     
+            
+            if any(strcmp(varargin,'terminal_states')) && obj.setup.J_term_terminal
+                pos = find(strcmp(varargin,'terminal_states'));
+                obj.setup.terminal_states = varargin{pos+1};
+            else
+                obj.setup.terminal_states = [];
             end
+            
+            if any(strcmp(varargin,'terminal_weights')) && obj.setup.J_term_terminal
+                pos = find(strcmp(varargin,'terminal_weights'));
+                obj.setup.terminal_weights = varargin{pos+1};
+            else
+                obj.setup.terminal_weights = ones(size(obj.setup.terminal_states));
+            end
+            
+            if any(strcmp(varargin,'terminal_normalise')) && obj.setup.J_term_terminal
+                pos = find(strcmp(varargin,'terminal_normalise'));
+                obj.setup.terminal_normalise = varargin{pos+1};
+            else
+                obj.setup.terminal_normalise = 0;
+            end
+            
             obj.setup.Nfilt = obj.setup.J_nterm-1;
             
             % option to define the safety interval in adaptive sampling
@@ -411,12 +630,11 @@
             % complete the params update in .setup
             obj.setup.params = params;
             
-            % number of reference trajectories (>1 for control design)
-            if (obj.setup.control_design)
-                obj.setup.Ntraj = params.Ntraj;
-            else
-                obj.setup.Ntraj = 1;
-            end
+            % number of reference trajectories            
+            obj.setup.Ntraj = params.Ntraj;
+            
+            % set the actual cost function (J)
+            obj.setup.cost_run = @obj.cost_function;
             
             % initialise the class
             obj = obj.obs_init();
@@ -430,6 +648,7 @@
             % get params structure. It's redundant but by doing so we save
             % the initial values of the params
             obj.init.params = obj.setup.params;
+            obj.init.params = obj.setup.params.params_update(obj.init.params,obj.setup.X_est(1).val(:,1));
             
             % get initial state
             obj.init.X = obj.setup.X;
@@ -439,10 +658,12 @@
             for traj=1:obj.setup.Ntraj
                 obj.init.X_filter(traj).val = cell(obj.setup.Nfilt,1);
                 obj.init.X_filter_est(traj).val = cell(obj.setup.Nfilt,1);
+                obj.init.X_filter_buffer_control(traj).val = cell(obj.setup.Nfilt,1);
                 for i=1:obj.setup.Nfilt
                     for dim=1:obj.setup.dim_out
                         obj.init.X_filter(traj).val{i,dim} = zeros(obj.setup.filterTF(i).dim,obj.setup.Niter);
-                        obj.init.X_filter_est(traj).val{i,dim} = zeros(obj.setup.filterTF(i).dim,obj.setup.Niter);
+                        obj.init.X_filter_est(traj).val{i,dim} = 0*randn(obj.setup.filterTF(i).dim,obj.setup.Niter);
+                        obj.init.X_filter_buffer_control(traj).val{i,dim} = zeros(obj.setup.filterTF(i).dim,obj.setup.Niter);
                     end
                 end
             end
@@ -466,6 +687,7 @@
             % implemented. 
             obj = obj.scale_factor();
             obj.init.normalised = 0;
+            obj.init.ConstrNormalised = 0;
             
             % Window Samples: maximum number of time instants considered in
             % the optimisation if fixed sampling is used. For more
@@ -482,7 +704,7 @@
             % persistenct excitation setup
             obj.init.c1_PE = 5;
             obj.init.d1_PE = 20;
-            
+            obj.init.PE_pos_array = [];
             for i=1:obj.setup.Ntraj
                 obj.init.buf_PE(i).val = zeros(1,obj.init.d1_PE);
             end
@@ -495,12 +717,20 @@
                 obj.init.Y_full_story(i).val = zeros(obj.setup.J_nterm,obj.setup.dim_out,0);
                 obj.init.Yhat_full_story(i).val = zeros(obj.setup.J_nterm,obj.setup.dim_out,0);
                 obj.init.target_story(i).val = zeros(obj.setup.J_nterm,obj.setup.dim_out,0);
+                % buffer for Y during drive computation (control design)
+                obj.init.Y_buffer_control(i).val = [];
+                obj.init.drive_out(i).val = [];
+                obj.init.input_story(i).val(:,1) = zeros(obj.setup.params.dim_input,1);
+                obj.init.input_story_ref(i).val(:,1) = zeros(obj.setup.params.dim_input,1);
             end
+            
+            % input dimension
+            obj.setup.dim_input = obj.setup.params.dim_input;
 
             % buffer adaptive sampling: these buffers keep track of the
             % time instants in which the measured data have been stored. 
             obj.init.Y_space = zeros(1,obj.setup.w);
-            obj.init.Y_space_full_story = 0;
+            obj.init.Y_space_full_story = 0;            
 
             % dJ condition buffer (adaptive sampling): numerical condition
             % to be thresholded in order to trigger the adaptive sampling.
@@ -516,6 +746,8 @@
             % J_components is used to keep track of the different cost
             % function terms amplitude. Not implemented in V1.1
             obj.init.J_components = ones(obj.setup.dim_out,obj.setup.J_nterm);
+            obj.init.just_optimised = 0;
+            obj.init.FirstOpt = 0;
             
             % time instants in which the optimisation is run
             obj.init.temp_time = [];
@@ -544,6 +776,7 @@
             % optimset: check documentation for fminsearch or fminunc
             obj.init.TolX = 0;
             obj.init.TolFun = 0;
+            obj.init.DiffMinChange = 1e-3;
             obj.init.last_opt_time = 0;
             obj.init.opt_time = 0;
             
@@ -555,23 +788,28 @@
             end
     
             
-            % optimset      
-            if strcmp(func2str(obj.setup.fmin),'fmincon')
-                obj.init.myoptioptions = optimoptions('fmincon', 'MaxIter', obj.setup.max_iter, 'display',obj.init.display, ...
-                                                      'OptimalityTolerance', 0, 'StepTolerance', 0,'MaxFunEvals',Inf, 'GradObj', 'off',...
-                                                      'OutputFcn',@obj.outfun,'TolFun',obj.init.TolFun,'TolX',obj.init.TolX);  
-            elseif strcmp(func2str(obj.setup.fmin),'fminunc')
-                obj.init.myoptioptions = optimoptions('fminunc', 'MaxIter', obj.setup.max_iter, 'display',obj.init.display, ...
-                                                      'OptimalityTolerance', 0, 'StepTolerance', 0,'MaxFunEvals',Inf, 'GradObj', 'off',...
-                                                      'OutputFcn',@obj.outfun,'TolFun',obj.init.TolFun,'TolX',obj.init.TolX);  
-            elseif strcmp(func2str(obj.setup.fmin),'fminsearch')
+            % optimset                    
+            if strcmp(func2str(obj.setup.fmin),'fminsearchcon')
                 obj.init.myoptioptions = optimset('MaxIter', obj.setup.max_iter, 'display',obj.init.display, ...
-                                                  'MaxFunEvals',Inf,...
-                                                  'OutputFcn',@obj.outfun,'TolFun',obj.init.TolFun,'TolX',obj.init.TolX); 
-            end                                              
+                                                  'MaxFunEvals',Inf,'OutputFcn',@obj.outfun,'TolFun',obj.init.TolFun,'TolX',obj.init.TolX);   
+            elseif strcmp(func2str(obj.setup.fmin),'patternsearch')                              
+                obj.init.myoptioptions = optimoptions(func2str(obj.setup.fmin), 'MaxIter', obj.setup.max_iter, 'display',obj.init.display, ...
+                                                      'Cache', 'on', 'UseParallel', false, 'StepTolerance', 0,'MaxFunEvals',Inf,'Algorithm','nups');            
+            else
+                obj.init.myoptioptions = optimoptions(func2str(obj.setup.fmin), 'MaxIter', obj.setup.max_iter, 'display',obj.init.display, ...
+                                                      'OptimalityTolerance', 0, 'StepTolerance', 0,'MaxFunEvals',Inf, 'GradObj', 'off',...
+                                                      'OutputFcn',@obj.outfun,'TolFun',obj.init.TolFun,'TolX',obj.init.TolX, ...
+                                                      'FiniteDifferenceStepSize', obj.init.DiffMinChange, 'FiniteDifferenceType','central');                          
+            end
             %%% end of optimisation setup %%%
 
         end
+
+        % nonlinear constraing default
+        function [c, ceq] = NONCOLcon(varargin)
+            c = 0;
+            ceq = 0;
+        end
         
         % scale factor: this method defines the cost function weights
         % accordingly to the selected filters (see setup.temp_scale).
@@ -585,23 +823,49 @@
         % shall be stopped or not. Check setup.J_thresh for more
         % information. 
         function stop = outfun(obj,x, optimValues, state)
-            if (optimValues.iteration == obj.setup.max_iter) || (optimValues.fval <= obj.init.TolFun)
+            if (optimValues.iteration == obj.setup.max_iter) || (optimValues.fval <= obj.init.TolFun) || obj.setup.MaxOptTimeFlag
                 stop = true;
             else
                 stop = false;
             end
         end
         
+        % convex conjugate: this function computes the convex conjugate of
+        % the cost function
+        function [J_final,x_final,obj] = convex_conjugate(obj,varargin)
+            
+            % get opt state
+            x_opt = varargin{1};           
+            
+            % define the function handle for the conjugate
+            y = ones(size(x_opt));
+            f_cc = @(x)-(transpose(y)*x - obj.cost_function(x,varargin{2},varargin{3},varargin{4}));
+            
+            % initial condition
+            x_0 = x_opt;
+            
+            % solve the sup problem to find the conjugate fcn
+            [x_final,J_final] = fminunc(f_cc,x_0,obj.init.myoptioptions);
+            
+            obj.init.convex_conjugate(end+1) = J_final;
+            
+        end                
+        
         % cost function: objective to be minimised by the MHE observer
         function [J_final,obj] = cost_function(obj,varargin) 
             
-            obj.init.t_J_start = tic;  
+            obj.init.t_J_start = tic;
             
             % above ntraj init
             J_final = 0;
+            J_input = 0;
                 
             for traj = 1:obj.setup.Ntraj
+                
+                obj.init.params.optimising = 1;
+                
                 obj.init.traj = traj;
+                obj.init.params.traj = traj;
                 
                 % cost function init
                 Jtot = 0;
@@ -638,9 +902,23 @@
                 buf_dist = diff(tspan_pos);
                 n_iter = sum(buf_dist);
                 % initial condition
-                x_start = x(1:obj.setup.dim_state);                
-                % get evolution with input
-                X = obj.setup.ode(@(t,x)obj.setup.model(t, x, obj.init.params, obj), tspan , x_start, obj.setup.odeset);
+                x_start = x(1:obj.setup.dim_state);      
+                % reset buffer for Y during drive computation (control design)
+                obj.init.Y_buffer_control(traj).val = [];
+                for i=1:obj.setup.Nfilt
+                    for dim=1:obj.setup.dim_out
+                        obj.init.X_filter_buffer_control(traj).val{i,dim} = 0*obj.init.X_filter_buffer_control(traj).val{i,dim};
+                    end
+                end
+                % get evolution with input only if at least 2 iinstans are
+                % considered
+                if length(tspan)>1
+                    X = obj.setup.ode(@(t,x)obj.setup.model(t, x, obj.init.params, obj), tspan , x_start, obj.setup.odeset);
+                else
+                    X.y = x_start;
+                end
+                
+                obj.init.params.optimising = 0;
                 
                 % check for NaN or Inf
                 NaN_Flag = find(isnan(X.y));
@@ -649,9 +927,17 @@
                     break
                 end
                 
-                %%% get measure  %%
-                Yhat = zeros(obj.setup.Nfilt+1,obj.setup.dim_out,size(X.y,2));
-                Yhat(1,:,:) = obj.setup.measure(X.y,obj.init.params,tspan);                                
+                Inf_Flag = isempty(isinf(X.y));
+                Huge_Flag = isempty(X.y>1e10);
+                if Inf_Flag || Huge_Flag
+                    J_final = Inf;
+                    break
+                end                
+                
+                %%% get measure  %%               
+                Yhat = zeros(obj.setup.Nfilt+1,obj.setup.dim_out,size(X.y,2));                
+                u_in = [zeros(size(obj.init.input_story(traj).val,1),1), obj.init.input_story(traj).val];
+                Yhat(1,:,:) = obj.setup.measure(X.y,obj.init.params,tspan,u_in(:,(tspan_pos(1):tspan_pos(end))),obj);
                 
                 %%% compute filters %%%
                 if obj.setup.Nfilt > 0                     
@@ -671,30 +957,34 @@
                            startpos = startpos + obj.setup.filterTF(filt).dim;
                        end
                     end
-                    
+                             
                     try
-                    [Y, ~] = obj.measure_filter(Yhat(:,:,tspan_filt),tspan_filt,x0_filter); 
-                    catch
-                       a=1; 
-                    end
+                        [Y, ~] = obj.measure_filter(Yhat(:,:,tspan_filt),tspan_filt,x0_filter);                         
+                    catch                        
+                        J_final = NaN;
+                        break
+                    end                    
+                    
                     for filt=1:obj.setup.Nfilt
                         for dim=1:obj.setup.dim_out
                             % odeDD
 %                             Yhat(filt+1,dim,tspan_filt(2:end)) = Y{filt,dim}.val;
-                            % Lsim
-                            Yhat(filt+1,dim,tspan_filt) = Y{filt,dim}.val(tspan_filt);
+                            % Lsim                            
+                            Yhat(filt+1,dim,tspan_filt) = Y{filt,dim}.val(tspan_filt);                               
                         end
                     end
                 end                                                                              
                 % cost function
-                J = zeros(obj.setup.J_nterm,size(Yhat,1));
+                J = zeros(obj.setup.J_nterm,length(obj.setup.dim_out_compare));
                 target_pos = find(obj.init.Y_space ~= 0);
 
                 for term=1:obj.setup.J_nterm
                     % get the J
                     for dim=1:obj.setup.dim_out
                         tspan_vals = tspan_pos(2:end) - tspan_pos(1) + 1;
-                        diff_var = reshape((y_target(term,dim,target_pos)-Yhat(term,dim,tspan_vals)),length(tspan_pos)-1,1);
+                        target_tmp = reshape((y_target(term,dim,target_pos)),length(tspan_pos)-1,1);
+                        hat_tmp = reshape(Yhat(term,dim,tspan_vals),length(tspan_pos)-1,1);
+                        diff_var = target_tmp-hat_tmp;
                         J(term,dim) = transpose(diff_var)*diff_var;
                     end
 
@@ -702,42 +992,78 @@
 
                 % scaling
                 J_scaled = zeros(size(J));
-                for dim=1:obj.setup.dim_out                            
-                    J_scaled(:,dim) = transpose(obj.init.scale_factor_scaled(dim,1:obj.setup.J_nterm)).*J(:,dim);
-                    Jtot = Jtot + sum(J_scaled(:,dim)); 
+                for dim=1:length(obj.setup.dim_out_compare)
+                    J_scaled(:,obj.setup.dim_out_compare(dim)) = transpose(obj.init.scale_factor_scaled(obj.setup.dim_out_compare(dim),1:obj.setup.J_nterm)).*J(:,obj.setup.dim_out_compare(dim));
+                    Jtot = Jtot + sum(J_scaled(:,obj.setup.dim_out_compare(dim))); 
                 end
 
                 % store terms
-                obj.init.Jterm_store(1:obj.setup.J_nterm) = obj.init.Jterm_store(1:obj.setup.J_nterm) + sum(J_scaled,2);
-
-                %%% spring like term %%%
-                if ~isempty(obj.setup.estimated_params) && obj.setup.J_term_spring
-                    x0 = obj.init.temp_x0(1).val;
-                    params0 = x0(obj.setup.estimated_params);
-                    paramsNow = x(obj.setup.estimated_params);
-                    paramsDiff = params0-paramsNow;
-                    paramsDiff = reshape(paramsDiff,1,length(obj.setup.estimated_params));
-                    Jspring = paramsDiff*obj.init.scale_factor(1,obj.setup.J_term_spring_position)*transpose(paramsDiff);
+                obj.init.Jterm_store(1:obj.setup.J_nterm) = obj.init.Jterm_store(1:obj.setup.J_nterm) + sum(J_scaled,2);                
+                
+                %%% terminal cost %%%
+                if obj.setup.J_term_terminal
+                    x0 = obj.init.temp_x0(obj.init.traj).val;                    
+                    xterm = x0(obj.setup.terminal_states)-x(obj.setup.terminal_states);
+                    paramsDiff = reshape(xterm,1,length(obj.setup.terminal_states));
+                    J_terminal = paramsDiff*diag(obj.init.scale_factor_scaled_terminal)*transpose(paramsDiff);
                     
                     % store terms
-                    obj.init.Jterm_store(end) = Jspring; 
+                    obj.init.Jterm_store(end) = J_terminal; 
                 else
-                    Jspring = 0;
+                    J_terminal = 0;
                 end
-                             
-                
-                % non opt vars barrier term
-                diff_var = x(obj.setup.nonopt_vars) - x_nonopt(obj.setup.nonopt_vars);
-                J_barr = 1e8*any(diff_var)*0;
+                                             
+                % non opt vars barrier term                
+                if obj.setup.bounds                    
+                    for bound=1:length(obj.setup.boundsPos)
+                        % init value
+                        init_value = obj.init.temp_x0(obj.init.traj).val(obj.setup.boundsPos(bound));
+                        % barrier - low
+                        err_low = min(X.y(obj.setup.boundsPos(bound),2:end)-obj.setup.boundsValLow(bound));
+                        % barrier - up
+                        err_up = max(obj.setup.boundsValUp(bound)-X.y(obj.setup.boundsPos(bound),2:end));
+                        % terminal
+                        err_terminal = norm(X.y(obj.setup.boundsPos(bound),:)-init_value);
+                        % sum stuff
+                        if (err_low > 0) && (err_up > 0)
+                            J_barr(bound) = 0*obj.setup.boundsWeight(bound)*err_terminal;
+                        else
+                            J_barr(bound) = 1e5;
+                        end
+                        
+                    end
+                    
+                else
+                    J_barr = 0;
+                end
+                                
+                if any(isinf(J_barr))
+                    J_final = Inf;
+                    break
+                end 
                 
-                J_final = J_final + Jtot + Jspring + J_barr;
-
-                %%% final stuff %%%
-                if n_iter > 0
-                    obj.init.Yhat_temp = Yhat;
+                %%% Allocation cost fucntion %%%
+                if 0
+                    u_diff = obj.init.input_story(traj).val;
+                    u_diff_norm = obj.init.params.Ru*vecnorm(u_diff).^2;                
+                    J_input = J_input + sum(u_diff_norm);
                 else
-                    J_final = 1; 
+                    J_input = 0;
+                end
+                                
+                J_final = J_final + Jtot + sum(J_barr) + J_terminal + 0*J_input;
+
+                %%% final stuff %%%                
+                obj.init.Yhat_temp = Yhat;
+                
+                currentTime = toc(obj.setup.opt_temp_time);
+                if currentTime > obj.setup.MaxOptTime
+                   J_final = 0;
+                   obj.setup.MaxOptTimeFlag = 1;
+                   return
                 end
+            
+
             end
             
             obj.init.t_J(end+1) = toc(obj.init.t_J_start);  
@@ -767,13 +1093,14 @@
                     for dim=1:obj.setup.dim_out 
                         
                         % init condition
-                        x0_filter = X_filter.val{nfilt,dim}(tspan_pos(1));                        
+                        x0_filter = X_filter.val{nfilt,dim}(:,tspan_pos(1));                        
 
                         % inputs
                         u = reshape(U(1,dim,tspan_pos),1,length(tspan_pos));
                         
                         if (min(diff(tspan_pos)) ~= 0)                                 
-                            [Y{nfilt,dim}.val, ~, X{nfilt,dim}.val] = lsim(obj.setup.filterTF(nfilt).TF,u',tspan,x0_filter);  
+                            [Y{nfilt,dim}.val, ~, tmp_xtraj] = lsim(obj.setup.filterTF(nfilt).TF,u',tspan,x0_filter);  
+                            X{nfilt,dim}.val = transpose(tmp_xtraj);
                         else
                             Y{nfilt,dim}.val = 0;
                             X{nfilt,dim}.val = x0_filter;
@@ -789,90 +1116,104 @@
             
         end
         
-        % dJ_cond: function for the adaptive sampling
-        function obj = dJ_cond_v5_function(obj)
-
-            buffer_ready = (nnz(obj.init.Y_space) >= 2);
-
-            if buffer_ready
-                
-                % init
-                temp_e = 0;
-                
-                for traj = 1:obj.setup.Ntraj
+        % dJ_cond: function for the adaptive sampling        
+        % it will simply return the main frequencies of the current signal
+        function obj = adaptive_sampling_function(obj)
+            
+            % Wavelet adaptive
+
+            % The buffer is ready when 
+            %   1) wavelet are considered
+            %   2) We have past the FNs*Buflen samples
+            % If you want to use the adaptive with the signal richness
+            % switch the PE_flag check
+            buffer_ready = (~obj.init.PE_flag)*(obj.init.ActualTimeIndex > obj.init.FNts*obj.init.Fbuflen);
+
+            % get the actual data
+            buf_data = squeeze(obj.init.Y(obj.init.traj).val(1,:,:));
+            y = obj.init.Y_full_story(obj.init.traj).val(1,:,obj.init.ActualTimeIndex);
+
+            % Here we compute the signal richness as in eq. (3.16)
+            DiffTerm = diff(buf_data);
+            VecTerm = vecnorm(DiffTerm,2,2);
+            obj.init.PE_story(:,obj.init.ActualTimeIndex) = sum(VecTerm) + norm(y'-buf_data(end,:));            
+
+            % Here we check if we're ready to do the adaptive sampling
+            if buffer_ready && obj.setup.AdaptiveSampling                
+
+                % get current buffer - the wavelet one
+                pos_hat = obj.init.ActualTimeIndex:-obj.init.FNts:(obj.init.ActualTimeIndex-obj.init.FNts*obj.init.Fbuflen);                
+                short_win_data = squeeze(obj.init.Y_full_story(obj.init.traj).val(1,obj.init.wavelet_output_dim,pos_hat));                
+
+                % set data                
+                short_win_data = reshape(short_win_data,numel(obj.init.wavelet_output_dim),numel(pos_hat));
+                buffer = vecnorm(short_win_data,2,1);                 
+                                                                                
+                % compute the wavelet transform                
+                [WT, ~] = cwt(buffer,obj.init.wvname,1/obj.setup.Ts,'VoicesPerOctave',obj.init.Nv);                
+
+                % get the frequency real values (they are imaginary from
+                % Fourier transform) and take the norm. This is a vector
+                % with all the frequencies indentifued in the buffer.
+                WT_real = real(WT);
+                WT_norm = vecnorm(WT_real,2,1);   
+                % We filter them with a moving average
+                WT_norm_filt = movmean(WT_norm,5);                                
+                % and we find the peaks, either them being related to the
+                % max or min. 
+                [WT_norm_peaks,pos_peaks] = findpeaks(WT_norm_filt);                
+
+                % Now we identify the max and min freq identified
+                if ~isempty(pos_peaks)
+                    
+                    % pos max and min on the WT
+                    [pos_max] = find(WT_norm_filt == max(WT_norm_peaks),1,'first');
+                    [pos_min] = find(WT_norm_filt == min(WT_norm_peaks),1,'first');
 
-                    % where to check
-                    [~, pos_hat] = find(obj.init.Y_space > 1);
-                    pos_hat = obj.init.Y_space(pos_hat)-1;
+                    % store max min F
+                    %obj.init.Fcwt_story(:,obj.init.ActualTimeIndex) = [F(pos_max) F(pos_min)];
 
-                    % Y measure - hat
-                    Yhat = obj.init.Yhat_full_story(traj).val(1,:,pos_hat);
-                    Yhat(1,:,end+1) = obj.init.Yhat_full_story(traj).val(1,:,obj.init.ActualTimeIndex);
-                    Yhat = reshape(Yhat,size(Yhat,2),size(Yhat,3));
-                                        
-                    % Y measure - real
-                    Y_buf = obj.init.Y_full_story(traj).val(1,:,pos_hat);
-                    Y_buf(1,:,end+1) = obj.init.Y_full_story(traj).val(1,:,obj.init.ActualTimeIndex);
-                    Y_buf = reshape(Y_buf,size(Y_buf,2),size(Y_buf,3));
-
-                    % build the condition
-                    n_sum = size(Y_buf,1);
-                    
-                    for i=1:n_sum
-                        temp_e = temp_e + norm(Y_buf(i,:)-Yhat(i,:)); 
-                    end                 
-                end
+                    % here we transform from rad/sec to Hz
+                    F_def(1,1) = 2*pi*WT_norm_filt(pos_max);  %1 max
+                    F_def(2,1) = 2*pi*WT_norm_filt(pos_min);  %2 min
+                       
+                    % Thresholding with the Fmin
+                    if min(F_def) < obj.init.Fmin
+                        F_def = zeros(obj.init.nfreqs,1);
+                    end
 
-                obj.init.dJ_cond = norm(temp_e);
-            else
-                obj.init.dJ_cond = 1.1*obj.setup.dJ_high;
-            end
-            
-            obj.init.dJ_cond_story(:,obj.init.ActualTimeIndex) = obj.init.dJ_cond;
-        end
-        
-        % Get Persistent Excitation and its derivative
-        function obj = PE(obj)
-            
-            for traj = 1:obj.setup.Ntraj
-                
-                nonzero_meas = nnz(obj.init.Y(traj).val(1,:,:));
-                
-                if (obj.setup.w > 1) 
-                    % Y measure
-                    Y_buf = obj.init.Y(traj).val(1,:,:);
-                    Y_buf(1,:,end+1) = obj.init.Y_full_story(traj).val(1,:,obj.init.ActualTimeIndex);
-                    Y_buf = reshape(Y_buf,size(Y_buf,2),size(Y_buf,3));
-
-                     %%% compute signal richness %%%             
-                     obj.init.PE(traj).val(obj.init.ActualTimeIndex) = sum(abs(diff(sum(Y_buf,1).^2)));
+                    % select freqs
+                    obj.init.freqs = [obj.init.freqs F_def];
                 else
-                     obj.init.PE(traj).val(obj.init.ActualTimeIndex) = 0;
-                end
-             
+                    obj.init.freqs = [obj.init.freqs zeros(obj.init.nfreqs,1)];
+                end                                                          
+            else
+                obj.init.freqs = [obj.init.freqs zeros(obj.init.nfreqs,1)];
+            end  
+
+            % If we selected the signal richness option, we proceed as from
+            % eq. (3.16)
+            if obj.init.PE_flag
+
+                % get data
+                buf_data = squeeze(obj.init.Y(obj.init.traj).val(1,:,:));
+                y = reshape(obj.init.Y_full_story(obj.init.traj).val(1,:,obj.init.ActualTimeIndex),obj.init.params.OutDim,1);
+                DiffTerm = diff(buf_data);
+                VecTerm = vecnorm(DiffTerm,2,2);
+                obj.init.PE_story(:,obj.init.ActualTimeIndex) = sum(VecTerm) + norm(y-buf_data(end,:));
+
+                % just pad with freqs info
+                obj.init.freqs = [obj.init.freqs ones(obj.init.nfreqs,1)];
             end
-             
-             obj.init.maxIterStory(obj.init.ActualTimeIndex) = obj.setup.max_iter;
-        end
+
+        end                
         
         % target function (observer or control design)
         function obj = target(obj)          
             for i=1:obj.setup.Ntraj
                 obj.init.target(i).val = obj.init.Y(i).val;
             end
-        end
-        
-        % control drive function (observer or control design)
-        function drive = drive(varargin) 
-            
-            obj = varargin{1};            
-            
-            % x - varargin
-            x_star = varargin{2}(1:obj.init.params.dim_state,:);            
-            x = varargin{3}(1:obj.init.params.dim_state,:);            
-            drive = x_star-x;
-            
-        end
+        end               
         
         % observer function: this method wraps up all the afromentioned
         % methods and actually implements the observer. Check the reference
@@ -893,31 +1234,30 @@
                 % save runtime state
                 obj.init.X_est_runtime(traj).val(:,obj.init.ActualTimeIndex) = obj.init.X_est(traj).val(:,obj.init.ActualTimeIndex);
                 % get ESTIMATED measure from ESTIMATED state (xhat)
-                yhat(traj).val = obj.setup.measure(xhat(traj).val,obj.init.params,obj.setup.time(obj.init.ActualTimeIndex));
+                yhat(traj).val = obj.setup.measure(xhat(traj).val,obj.init.params,obj.setup.time(obj.init.ActualTimeIndex),obj.init.input_story(traj).val(:,max(1,obj.init.ActualTimeIndex-1)),obj);
             end
             
             for traj=1:obj.setup.Ntraj
                 obj.init.traj = traj;
                 % get filters - y
-                obj.init.Y_full_story(traj).val(1,:,obj.init.ActualTimeIndex) = y(traj).val; 
+                obj.init.Y_full_story(traj).val(1,:,obj.init.ActualTimeIndex) = y(traj).val;                 
                 tspan_pos = [max(1,obj.init.ActualTimeIndex-1), obj.init.ActualTimeIndex];
                 [dy, x_filter] = obj.measure_filter(obj.init.Y_full_story(traj).val,tspan_pos,obj.init.X_filter(obj.init.traj));                
                 for filt=1:obj.setup.Nfilt
                     for dim=1:obj.setup.dim_out
-                        y_tmp = dy{filt,dim}.val(end);
-                        obj.init.X_filter(traj).val{filt,dim}(:,tspan_pos) = x_filter{filt,dim}.val;
+                        y_tmp(dim,1) = dy{filt,dim}.val(end);
+                        obj.init.X_filter(traj).val{filt,dim}(:,unique(tspan_pos)) = x_filter{filt,dim}.val;
                     end   
                     y(traj).val = [y(traj).val, y_tmp];
                 end                
-                
                 % get filters - yhat
                 obj.init.Yhat_full_story(traj).val(1,:,obj.init.ActualTimeIndex) = yhat(traj).val;
                 tspan_pos = [max(1,obj.init.ActualTimeIndex-1), obj.init.ActualTimeIndex];
                 [dyhat, x_filter] = obj.measure_filter(obj.init.Yhat_full_story(traj).val,tspan_pos,obj.init.X_filter_est(obj.init.traj));
                 for filt=1:obj.setup.J_nterm-1
                     for dim=1:obj.setup.dim_out
-                        y_tmp = dyhat{filt,dim}.val(end);                        
-                        obj.init.X_filter_est(traj).val{filt,dim}(:,tspan_pos) = x_filter{filt,dim}.val;
+                        y_tmp(dim,1) = dyhat{filt,dim}.val(end);                        
+                        obj.init.X_filter_est(traj).val{filt,dim}(:,unique(tspan_pos)) = x_filter{filt,dim}.val;
                     end
                     yhat(traj).val = [yhat(traj).val, y_tmp];                    
                 end
@@ -927,35 +1267,113 @@
             for term=1:obj.setup.J_nterm
                 for traj=1:obj.setup.Ntraj
                     obj.init.traj = traj;
-                    obj.init.Y_full_story(traj).val(term,:,obj.init.ActualTimeIndex) = y(traj).val(:,term);
+                    obj.init.Y_full_story(traj).val(term,:,obj.init.ActualTimeIndex) = y(traj).val(:,term);                    
                     obj.init.Yhat_full_story(traj).val(term,:,obj.init.ActualTimeIndex) = yhat(traj).val(:,term);
                     obj.init.Yhat_full_story_runtime(traj).val(term,:,obj.init.ActualTimeIndex) = yhat(traj).val(:,term);
                 end
-            end
+            end                        
 
             % fisrt bunch of data - read Y every Nts and check if the signal is
-            distance = obj.init.ActualTimeIndex-obj.init.Y_space(end);
+            distance = obj.init.ActualTimeIndex-obj.init.Y_space(end);   
+            NtsPos = mod(obj.init.Nsaved,obj.setup.w)+1;
+            obj.init.NtsPos = NtsPos;
+
+            % if the current Nts is different from the standard one, than
+            % it measn that a complete cycle has been done, and you should
+            % replace it again with the original one
+            if obj.setup.NtsVal(NtsPos) ~= obj.setup.Nts
+                obj.setup.NtsVal(NtsPos) = obj.setup.Nts;
+            end
+            
+            % set optimization time and iterations depending on the current Nts
+            if obj.setup.NtsVal(NtsPos) == min(obj.setup.NtsVal)
+                obj.setup.max_iter = min(obj.setup.max_iterVal);
+                if ~isinf(obj.setup.MaxOptTime)
+                    obj.setup.MaxOptTime = 0.2*obj.setup.NtsVal(NtsPos)*obj.setup.Ts;
+                end
+            else
+                obj.setup.max_iter = max(obj.setup.max_iterVal);
+                if ~isinf(obj.setup.MaxOptTime)
+                    obj.setup.MaxOptTime = 0.2*obj.setup.NtsVal(NtsPos)*obj.setup.Ts;
+                end
+            end            
             
-            obj = obj.dJ_cond_v5_function();
-            obj = obj.PE();
+            % Call the adaptive sampling function. It will set either the
+            % signal richness or the wavelet freqs
+            obj = obj.adaptive_sampling_function();
+
+            % define selcted freq: freq_sel=1 MAX freq_sel=2 MIN
+            freq_sel = obj.init.Fselect;
+
+            % define bound on freq (at least 2 due to Nyquist)
+            freq_bound = obj.init.Fnyq;
+
+            % set NtsVal depending on freqs
+            if obj.setup.AdaptiveSampling
+
+                % define the last estimation error
+                lasterr = nonzeros(squeeze(obj.init.Y(obj.init.traj).val(1,:,:))) - nonzeros(squeeze(obj.init.Yhat_full_story(obj.init.traj).val(1,:,nonzeros(obj.init.Y_space))));
+                if ~isempty(lasterr)
+                    lasterr = vecnorm(lasterr);
+                else
+                    lasterr = Inf;
+                end
+                
+                % store
+                obj.init.lasterror_story(obj.init.traj).val(obj.init.ActualTimeIndex) = lasterr;
+
+                % here with wavelets
+                if (~obj.init.PE_flag)
+
+                    % if I'm above the epsilon Delta, I check the sampling
+                    if (lasterr > obj.init.epsD)                        
+                        % Here we implement eq. (3.32-3.331-3.33b)
+                        freq = freq_bound*obj.init.freqs(freq_sel,end); % Hz
+                        Ts_wv = 1/(freq); % s
+                        distance_min = max(obj.init.FNts,ceil(Ts_wv/obj.setup.Ts));
+                    else
+                        distance_min = Inf;
+                    end
+                end
+
+                % here with signal richness
+                if (obj.init.PE_flag)
+                    if (obj.init.PE_story(obj.init.ActualTimeIndex) >= freq_bound) && (lasterr > obj.init.epsD)
+                        distance_min = distance;
+                    else
+                        distance_min = Inf;
+                    end
+                end
+
+                % If no freqs were set, don't sample
+                if ~any(obj.init.freqs(:,end)) 
+                    distance_min = Inf;                
+                end
+
+                % store the dmin
+                obj.init.dmin_story(obj.init.ActualTimeIndex) = distance_min;
+
+            else
+                % as expected from Nts
+                distance_min = obj.setup.NtsVal(NtsPos);
+            end
+
+            % update the minimum distance                        
+            obj.setup.NtsVal(NtsPos) = distance_min;            
+            
+            % safety flag
             obj.init.distance_safe_flag = (distance < obj.init.safety_interval);
-            %%%% select optimisation with hystheresis - dJcond %%%%%
-            hyst_low = (obj.init.dJ_cond_story(max(1,obj.init.ActualTimeIndex-1)) < obj.setup.dJ_low) && (obj.init.dJ_cond >= obj.setup.dJ_low);
-            hyst_high = (obj.init.dJ_cond >= obj.setup.dJ_high);
-            % flag = all good, no sampling
-            obj.init.hyst_flag = ~(hyst_low || hyst_high);
-            %%%% select optimisation with hystheresis - PE %%%%%
-            % flag = all good, no sampling
-            obj.init.PE_flag = obj.init.PE(traj).val(obj.init.ActualTimeIndex) <= obj.setup.dPE;
+            obj.init.distance_safe_flag_story(obj.init.traj).val(obj.init.ActualTimeIndex) = obj.init.distance_safe_flag;
+            
             %%%% observer %%%%
-            if  ( ~( ( (distance < obj.setup.Nts) || obj.init.hyst_flag || obj.init.PE_flag ) && (obj.init.distance_safe_flag) ) ) && (obj.setup.optimise)
+            if  ( ~( (distance < obj.setup.NtsVal(NtsPos)) && (obj.init.distance_safe_flag) ) )
 
                 if obj.setup.print
                     % Display iteration slengthtep
                     disp(['n window: ', num2str(obj.setup.w),'  n samples: ', num2str(obj.setup.Nts)])                    
                     disp(['N. optimisations RUN: ',num2str(obj.init.opt_counter)]);
-                    disp(['N. optimisations SELECTED: ',num2str(obj.init.select_counter)]);
-                end
+                    disp(['N. optimisations SELECTED: ',num2str(obj.init.select_counter)]);                    
+                end                                
                
                 %%%% OUTPUT measurements - buffer of w elements
                 % measures
@@ -967,30 +1385,57 @@
                     end
                 end
 
-                % adaptive sampling
+                % adaptive sampling                
                 obj.init.Y_space(1:end-1) = obj.init.Y_space(2:end);
                 obj.init.Y_space(end) = obj.init.ActualTimeIndex;
                 obj.init.Y_space_full_story(end+1) = obj.init.ActualTimeIndex;
-
+                
+                obj.init.Nsaved = obj.init.Nsaved + 1;
+                
                 % store measure times
                 obj.init.temp_time = [obj.init.temp_time obj.init.ActualTimeIndex];
 
                 % check only on the first traj as the sampling is coherent
                 % on the 2.
-                if obj.setup.control_design == 0
+                % WaitAlBuffer == 0 --> start asap
+                if obj.setup.WaitAllBuffer == 0
                     cols_nonzeros = length(find(obj.init.Y_space ~= 0))*obj.setup.dim_out*nnz(obj.setup.J_temp_scale);                
-                else
+                % WaitAlBuffer == 1 --> start when all the buffer is full
+                elseif obj.setup.WaitAllBuffer == 1
                     cols_nonzeros = length(find(obj.init.Y_space ~= 0));  
+                % WaitAlBuffer == 2 --> start at the last step of the trajectory
+                elseif obj.setup.WaitAllBuffer == 2
+                    cols_nonzeros = obj.setup.Nts;
+                else
+                    error('Wrong Wait Buffer Option')
                 end
 
                 % flag
-                if obj.setup.control_design == 0
+                % WaitAlBuffer == 0 --> start asap
+                if obj.setup.WaitAllBuffer == 0
                     flag = 2*length(obj.setup.opt_vars)+1; % Aeyels condition (see https://doi.org/10.48550/arXiv.2204.09359)
-                else
+                % WaitAlBuffer == 1 --> start when all the buffer is full
+                elseif obj.setup.WaitAllBuffer == 1
                     flag = obj.setup.w;
+                % WaitAlBuffer == 2 --> start at the last step of the trajectory
+                elseif obj.setup.WaitAllBuffer == 2
+                    flag = obj.setup.Niter-obj.init.ActualTimeIndex;
+                else
+                    error('Wrong Wait Buffer Option')
                 end
                 % real
-                if cols_nonzeros >= flag
+                if cols_nonzeros >= flag                                        
+
+                    if obj.setup.WaitAllBuffer == 2
+                        obj.setup.Niter = obj.init.ActualTimeIndex;
+                        obj.setup.time = obj.setup.time(1:obj.init.ActualTimeIndex);
+                        obj.init.break = 1;
+                        obj.setup.w = numel(obj.init.Y_space_full_story)-1;
+                        obj.init.Y_space = nonzeros(obj.init.Y_space_full_story)';
+                        for traj=1:obj.init.params.Ntraj
+                            obj.init.Y(traj).val = obj.init.Y_full_story(traj).val(:,:,obj.init.Y_space);
+                        end                        
+                    end
 
                     if obj.setup.forward
 
@@ -1011,7 +1456,7 @@
                             
                             % buffer adaptive sampling: these buffers keep track of the
                             % time instants in which the measured data have been stored. 
-                            step = 2*obj.setup.Nts;
+                            step = 1*obj.setup.Nts;
                             start = obj.init.Y_space(end)-step*(obj.setup.w-1); 
                             stop = obj.init.Y_space(end);
                             obj.init.Y_space = start:step:stop;
@@ -1027,23 +1472,25 @@
                             obj.init.Y_space_full_story = [obj.init.Y_space_full_story, obj.init.Y_space];
                             
                             % set again (used for back_time)
-                            buf_Y_space_full_story = obj.init.Y_space_full_story(end-obj.setup.w+1:end);
+                            buf_Y_space_full_story = obj.init.Y_space_full_story(end-1*obj.setup.w+1:end);
+                            
+                            % back time index
+                            buf_dist = diff(buf_Y_space_full_story);                        
+                            obj.init.BackTimeIndex = obj.setup.time(max(obj.init.ActualTimeIndex-sum(buf_dist(1:end)),1));
+                        else                            
+                            % back time index
+                            buf_dist = diff(buf_Y_space_full_story);
+                            obj.init.BackTimeIndex = obj.setup.time(max(obj.init.ActualTimeIndex-sum(buf_dist(1:end)),1)); 
                         end
                         
-                        % back time index
-                        buf_dist = diff(buf_Y_space_full_story);
-                        obj.init.BackTimeIndex = obj.setup.time(max(obj.init.ActualTimeIndex-sum(buf_dist),1)); 
-                        obj.init.BackIterIndex = find(obj.setup.time==obj.init.BackTimeIndex);
+                        
+                        obj.init.BackIterIndex = find(obj.setup.time==obj.init.BackTimeIndex);                        
 
                         % set of initial conditions
                         for traj=1:obj.setup.Ntraj
                             obj.init.traj = traj;
-                            % start from the correct one
-                            if obj.setup.control_design 
-                                obj.init.temp_x0_nonopt(traj).val = obj.init.X_est(traj).val(obj.setup.nonopt_vars,obj.init.BackIterIndex);
-                            else
-                                obj.init.temp_x0_nonopt(traj).val = obj.init.X(traj).val(obj.setup.nonopt_vars,obj.init.BackIterIndex);
-                            end
+                            % start from the correct one                             
+                            obj.init.temp_x0_nonopt(traj).val = obj.init.X_est(traj).val(obj.setup.nonopt_vars,obj.init.BackIterIndex);
                             % change only the values which are to be optimised
                             % only 1 set of vars regardless to the number
                             % of trajectories used as we're not estimating
@@ -1056,14 +1503,13 @@
                             filterstartpos = max(1,obj.init.BackIterIndex);
                             for nfilt=1:obj.setup.Nfilt
                                 for dim=1:obj.setup.dim_out
-                                    tmp = reshape(obj.init.X_filter(traj).val{nfilt,dim}(:,filterstartpos),1,obj.setup.filterTF(nfilt).dim);
+                                    tmp = reshape(obj.init.X_filter(traj).val{nfilt,dim}(:,filterstartpos),1,obj.setup.filterTF(nfilt).dim);                                    
                                     x0_filters = [x0_filters, tmp];
                                 end
                             end
                             obj.init.temp_x0_filters(traj).val = x0_filters;
                             Lfilt = length(x0_filters);
                              
-
                             % reconstruct temp_x0 from opt/nonopt vars
                             obj.init.temp_x0(traj).val = zeros(obj.setup.dim_state,1);
                             obj.init.temp_x0(traj).val(obj.setup.opt_vars) = obj.init.temp_x0_opt;
@@ -1081,352 +1527,317 @@
                         end
                         
                         %%% normalisation %%%
-                        if 1 && (~obj.init.normalised) 
+                        if (obj.setup.J_normalise) && (~obj.init.normalised) 
                             range = 1:obj.init.ActualTimeIndex;
                             for filt=1:obj.setup.J_nterm
-                                for dim=1:obj.setup.dim_out
-                                    E = (obj.init.Yhat_full_story(traj).val(filt,dim,range) - obj.init.Y_full_story(traj).val(filt,dim,range)).^2;
-                                    E = reshape(E,1,size(E,3));
-                                    Emax = max(E);
-                                    if Emax == 0
-                                        Emax = 1;
+                                for dim=1:length(obj.setup.dim_out_compare)
+                                    for traj=1:obj.setup.Ntraj
+                                        E = (obj.init.Yhat_full_story(traj).val(filt,obj.setup.dim_out_compare(dim),range) - ...
+                                            obj.init.Y_full_story(traj).val(filt,obj.setup.dim_out_compare(dim),range)).^2;
+                                        E = reshape(E,1,size(E,3));
+                                        Emax(traj) = max(E);
+                                        if Emax(traj) == 0 || (abs(Emax(traj)) < 1e-15)
+                                            Emax(traj) = 1;
+                                        end                                        
                                     end
-                                    obj.init.scale_factor_scaled(dim,filt) = obj.init.scale_factor(dim,filt)/Emax;
+                                    Emax = max(Emax);
+                                    obj.init.scale_factor_scaled(obj.setup.dim_out_compare(dim),filt) = obj.init.scale_factor(obj.setup.dim_out_compare(dim),filt)/Emax;
+                                end
+                            end        
+                            if (obj.setup.J_term_terminal) && (obj.setup.terminal_normalise)
+                                E = vecnorm(obj.init.X_est(traj).val(obj.setup.terminal_states,range)');
+                                E_scale = E/sum(E);
+                                for dim=1:length(obj.setup.terminal_states)                                                                        
+%                                     obj.init.scale_factor_scaled_terminal(dim) = obj.init.scale_factor(1,obj.setup.J_term_terminal_position)/(E_scale(dim));                                    
+                                    obj.init.scale_factor_scaled_terminal(dim) = obj.init.scale_factor(1,obj.setup.J_term_terminal_position)/E(dim);
+                                end    
+                            elseif (obj.setup.J_term_terminal)
+                                for dim=1:length(obj.setup.terminal_states)
+                                    obj.init.scale_factor_scaled_terminal(dim) = obj.init.scale_factor(1,obj.setup.J_term_terminal_position);
+                                end
+                            end   
+                            if (~isempty(obj.setup.terminal_weights)) && (obj.setup.J_term_terminal)
+                                for dim=1:length(obj.setup.terminal_states)
+                                    obj.init.scale_factor_scaled_terminal(dim) = obj.init.scale_factor_scaled_terminal(dim)*obj.setup.terminal_weights(dim);
                                 end
-                            end                            
+                            end
                             obj.init.normalised = 1;
-                        end                       
-                        
-                        % save J before the optimisation to confront it later 
-                        [J_before, obj_tmp] = obj.cost_function(obj.init.temp_x0_opt,obj.init.temp_x0_nonopt,obj.init.temp_x0_filters,obj.init.target);  
-
-                        
-                        % Optimisation (only if distance_safe_flag == 1)
-                        opt_time = tic;
-                        
-                        %%%%% OPTIMISATION - NORMAL MODE %%%%%%
-                        if strcmp(func2str(obj.setup.fmin),'fmincon')                                                       
-                            [NewXopt, J, obj.init.exitflag,output] = obj.setup.fmin(@(x)obj.cost_function(x,obj.init.temp_x0_nonopt,obj.init.temp_x0_filters,obj.init.target,1),...
-                                                                     obj.init.temp_x0_opt, obj.setup.Acon, obj.setup.Bcon,obj.setup.Acon_eq, obj.setup.Bcon_eq, obj.setup.LBcon,...
-                                                                     obj.setup.UBcon, obj.setup.NONCOLcon, obj.init.myoptioptions);
-                        else
-                            [NewXopt, J, obj.init.exitflag,output] = obj.setup.fmin(@(x)obj.cost_function(x,obj.init.temp_x0_nonopt,obj.init.temp_x0_filters,obj.init.target,1),...
-                                                                     obj.init.temp_x0_opt, obj.init.myoptioptions);
+                        elseif (~obj.setup.J_normalise)
+                            obj.init.scale_factor_scaled = obj.init.scale_factor;
+                            if obj.setup.J_term_terminal
+                                for dim=1:length(obj.setup.terminal_states)
+                                    obj.init.scale_factor_scaled_terminal(dim) = obj.init.scale_factor(1,obj.setup.J_term_terminal_position)*obj.setup.terminal_weights(dim);
+                                end
+                            end
                         end
                         
-                        % save numer of iterations
-                        obj.init.NiterFmin(obj.init.ActualTimeIndex) = output.iterations;
-                        obj.init.exitflag_story(obj.init.ActualTimeIndex) = obj.init.exitflag;
+                        % check fmin time (boundaries)
+                        obj.setup.opt_temp_time = tic;
+                        obj.setup.MaxOptTimeFlag = 0;
                         
-                        % reconstruct NewXopt from opt/nonopt vars
-                        NewXopt_tmp = [];
-                        for traj = 1:obj.setup.Ntraj
-                            obj.init.traj = traj;
-                            NewXopt_end = zeros(obj.setup.dim_state,1);
-                            NewXopt_end(obj.setup.opt_vars) = NewXopt;
-                            NewXopt_end(obj.setup.nonopt_vars) = obj.init.temp_x0_nonopt(traj).val;                          
-                            NewXopt_tmp(traj).val = NewXopt_end;                           
-                        end
-                        NewXopt = NewXopt_tmp;
-
-                        % opt counter
-                        if traj == 1
-                            obj.init.opt_counter = obj.init.opt_counter + 1;
-                        end
-
-                        % check J_dot condition
-                        J_diff = (J/J_before);
-
-                        if (obj.setup.AlwaysOpt) || ( (J_diff <= obj.setup.Jdot_thresh) || (distance > obj.init.safety_interval) )
+                        % save max times
+                        obj.init.MaxOptTime_story = [obj.init.MaxOptTime_story obj.setup.MaxOptTime];
+                        obj.init.MaxIter_story = [obj.init.MaxIter_story obj.setup.max_iter];
+                        
+                        if (obj.setup.optimise)
                             
-                            % update params
-                            obj.init.params = obj.setup.params.params_update(obj.init.params,NewXopt(1).val);
-
-                            % on each trajectory
-                            for traj=1:obj.setup.Ntraj
-                                obj.init.traj = traj;
-                                % update
-                                obj.init.X_est(traj).val(:,obj.init.BackIterIndex) = NewXopt(traj).val;
-                                % store measure times
-                                obj.init.opt_chosen_time = [obj.init.opt_chosen_time obj.init.ActualTimeIndex];
-                                obj.init.just_optimised = 1;
-
-                                % counters
-                                obj.init.jump_flag = 0;
-                                obj.init.select_counter = obj.init.select_counter + 1;
-
-                                x_propagate = NewXopt(traj).val;
-
-                                %%%%%%%%%%%%%%%%% FIRST MEASURE UPDATE %%%%%%%%
-                                % manage measurements
-                                back_time = obj.init.BackIterIndex;
-
-                                %%%% ESTIMATED measurements
-                                % measures       
-                                % NB: the output storage has to be done in
-                                % back_time+1 as the propagation has been
-                                % performed 
-                                Yhat = obj.setup.measure(x_propagate,obj.init.params,obj.setup.time(back_time));
-                                % get filters - yhat
-                                obj.init.Yhat_full_story(traj).val(1,:,back_time) = Yhat;  
-                                tspan_pos = [max(1,back_time-1), back_time];
-                                [dyhat, x_filter] = obj.measure_filter(obj.init.Yhat_full_story(traj).val,tspan_pos,obj.init.X_filter_est(obj.init.traj));                                
-                                yhat(traj).val = Yhat;
-                                for filt=1:obj.setup.Nfilt
-                                    for dim=1:obj.setup.dim_out
-                                        y_tmp = dyhat{filt,dim}.val(end);
-                                        obj.init.X_filter_est(traj).val{filt,dim}(:,tspan_pos) = x_filter{filt,dim}.val;
-                                    end
-                                    yhat(traj).val = [yhat(traj).val, y_tmp];
+                            % flag for first opt
+                            obj.init.FirstOpt = 1;
+                            
+                            % save J before the optimisation to confront it later 
+                            [J_before, obj_tmp] = obj.setup.cost_run(obj.init.temp_x0_opt,obj.init.temp_x0_nonopt,obj.init.temp_x0_filters,obj.init.target);
+
+
+                            % Optimisation (only if distance_safe_flag == 1)
+                            opt_time = tic;                        
+
+                            %%%%% OPTIMISATION - NORMAL MODE %%%%%%                            
+                            % run optimization
+                            if obj.setup.MultiStart                                                                 
+                                % init multistart
+                                ms = MultiStart('FunctionTolerance',obj.init.TolFun, 'XTolerance', obj.init.TolX, 'UseParallel',false);
+                            end                                                                   
+                            % create problem
+                            try
+                                problem = createOptimProblem(func2str(obj.setup.fmin),'objective',@(x)obj.setup.cost_run(x,obj.init.temp_x0_nonopt,obj.init.temp_x0_filters,obj.init.target,1),...
+                                                                            'x0', obj.init.temp_x0_opt, 'Aineq', obj.setup.Acon, 'bineq', obj.setup.Bcon, 'Aeq', obj.setup.Acon_eq, 'beq', obj.setup.Bcon_eq, ...
+                                                                            'lb', obj.setup.LBcon, 'ub', obj.setup.UBcon, 'nonlcon', @(x)obj.setup.NONCOLcon(x,obj.init.temp_x0_nonopt,obj), 'options', obj.init.myoptioptions);
+                                if ~obj.setup.MultiStart
+                                    [NewXopt, J] = obj.setup.fmin(problem);
+                                else
+                                    [NewXopt, J] = run(ms,problem,obj.init.params.tpoints);                                                                
                                 end
-                                   
                                 
-                                for term=1:obj.setup.J_nterm
-                                    obj.init.Yhat_full_story(traj).val(term,:,back_time) = yhat(traj).val(:,term);
+                            catch
+                                problem = createOptimProblem('fmincon','objective',@(x)obj.setup.cost_run(x,obj.init.temp_x0_nonopt,obj.init.temp_x0_filters,obj.init.target,1),...
+                                                                            'x0', obj.init.temp_x0_opt, 'Aineq', obj.setup.Acon, 'bineq', obj.setup.Bcon, 'Aeq', obj.setup.Acon_eq, 'beq', obj.setup.Bcon_eq, ...
+                                                                            'lb', obj.setup.LBcon, 'ub', obj.setup.UBcon, 'nonlcon', @(x)obj.setup.NONCOLcon(x,obj.init.temp_x0_nonopt,obj), 'options', obj.init.myoptioptions);
+                                                                        
+                                if strcmp(func2str(obj.setup.fmin),'patternsearch')                                                             
+                                    problem.solver = 'patternsearch';   
+                                    obj.init.myoptioptions.ConstraintTolerance = 1e-10;
+                                    obj.init.myoptioptions.ScaleMesh = 'off';
+                                    obj.init.myoptioptions.MaxMeshSize = 100;
+                                    obj.init.myoptioptions.InitialMeshSize = 100;
+                                    obj.init.myoptioptions.Display = 'iter';
+                                    obj.init.myoptioptions.Algorithm = 'nups-gps';
+                                    obj.init.myoptioptions.UseParallel = true;                                    
+                                    obj.init.myoptimoptions.UseCompletePoll = true;
+                                elseif strcmp(func2str(obj.setup.fmin),'fminsearchcon')   
+                                    problem = createOptimProblem('fmincon','objective',@(x)obj.setup.cost_run(x,obj.init.temp_x0_nonopt,obj.init.temp_x0_filters,obj.init.target,1),...
+                                                                            'x0',  obj.init.temp_x0_opt, 'lb', obj.setup.LBcon, 'ub', obj.setup.UBcon, 'Aeq', obj.setup.Acon_eq, 'beq', obj.setup.Bcon_eq, ...
+                                                                            'nonlcon', @(x)obj.setup.NONCOLcon(x,obj.init.temp_x0_nonopt,obj), 'options', obj.init.myoptioptions);
+                                    problem = rmfield(problem,'bineq');
+                                    problem = rmfield(problem,'Aineq');   
+                                    obj.init.myoptioptions.ConstraintTolerance = 1e-10;
+                                    problem.options = obj.init.myoptioptions;
+                                    problem.solver = 'fminsearchcon';                                    
+                                else    
+                                    error('WRONG OPTIMISATION SETUP');
                                 end
-                                %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-                                %%%%%%%%%%% PROPAGATION %%%%%%%%%%%%%%%%%%%%%%%
-                                n_iter_propagate = obj.init.ActualTimeIndex-obj.init.BackIterIndex;
-
-                                for j =1:n_iter_propagate                                    
-                                    
-                                    % back time
-                                    back_time = obj.init.BackIterIndex+j;
-                                    tspan = obj.setup.time(back_time-1:back_time);
-                                    t = tspan(2);
-                                    
-                                    % how do you handle the input?
-                                    obj.init.params.ActualTimeIndex = back_time; % here you have the -1 because BackIterIndex is differently set up than in measure_function                                                                         
-
-                                    % integrate
-                                    obj.init.t_ode_start = tic;                     
-                                    X = obj.setup.ode(@(t,x)obj.setup.model(t, x, obj.init.params, obj), tspan, x_propagate,obj.setup.odeset);                                    
-                                    x_propagate = X.y(:,end);                      
-                                    obj.init.X_est(traj).val(:,back_time) = x_propagate;
-                                    obj.init.t_ode(end+1) = toc(obj.init.t_ode_start);
+                                
+                                problem.options = obj.init.myoptioptions;  
+                                
+                                if ~obj.setup.MultiStart
+                                    try
+                                        [NewXopt, J] = obj.setup.fmin(problem);
+                                    catch ME
+                                        [NewXopt, J] = obj.setup.fmin(problem.objective,problem.x0,problem.lb,problem.ub,problem.Aeq,problem.beq,problem.nonlcon,problem.options);
+                                    end
+                                else
+                                    list = obj.init.params.tpoints.list;
+                                    for pp = 1:obj.init.params.tpoints.NumStartPoints
+                                        problem.x0 = list(pp,:);
+                                        [J_before(pp), obj_tmp] = obj.setup.cost_run(problem.x0,obj.init.temp_x0_nonopt,obj.init.temp_x0_filters,obj.init.target);   
+                                        try
+                                            [NewXopt(pp,:), J(pp,:)] = obj.setup.fmin(problem);
+                                        catch ME
+                                            [NewXopt(pp,:), J(pp,:)] = obj.setup.fmin(problem.objective,problem.x0,problem.lb,problem.ub,problem.Aeq,problem.beq,problem.nonlcon,problem.options);
+                                        end
+                                    end
+                                    %J_improve = J./J_before';
+                                    [J,pos] = min(J);
+                                    J_before = J_before(pos);
+                                    NewXopt = NewXopt(pos,:);
+                                end
+                            end
+                            
+                            % reconstruct NewXopt from opt/nonopt vars
+                            NewXopt_tmp = [];
+                            for traj = 1:obj.setup.Ntraj
+                                obj.init.traj = traj;
+                                NewXopt_end = zeros(obj.setup.dim_state,1);
+                                NewXopt_end(obj.setup.opt_vars) = NewXopt;
+                                NewXopt_end(obj.setup.nonopt_vars) = obj.init.temp_x0_nonopt(traj).val;                          
+                                NewXopt_tmp(traj).val = NewXopt_end;                           
+                            end
+                            % set new state
+                            NewXopt = NewXopt_tmp;                                                
+
+                            % opt counter
+                            if traj == 1
+                                obj.init.opt_counter = obj.init.opt_counter + 1;
+                            end
+
+                            % check J_dot condition
+                            J_diff = (J/J_before);
+
+                            if (obj.setup.AlwaysOpt) || ( (J_diff <= obj.setup.Jdot_thresh) || (distance > obj.init.safety_interval) )                                                        
+
+                                % on each trajectory
+                                for traj=1:obj.setup.Ntraj
+
+                                    % set traj
+                                    obj.init.traj = traj;
+
+                                    % update state
+                                    obj.init.X_est(traj).val(:,obj.init.BackIterIndex) = NewXopt(traj).val;
+
+                                    % store measure times
+                                    obj.init.opt_chosen_time = [obj.init.opt_chosen_time obj.init.ActualTimeIndex];
+                                    obj.init.just_optimised = 1;
+
+                                    % counters
+                                    obj.init.jump_flag = 0;
+                                    obj.init.select_counter = obj.init.select_counter + 1;
+
+                                    x_propagate = NewXopt(traj).val;
+
+                                    % update params
+                                    obj.init.params = obj.setup.params.params_update(obj.init.params,x_propagate);
+
+                                    %%%%%%%%%%%%%%%%% FIRST MEASURE UPDATE %%%%%%%%
+                                    % manage measurements
+                                    back_time = obj.init.BackIterIndex;
 
                                     %%%% ESTIMATED measurements
                                     % measures       
                                     % NB: the output storage has to be done in
                                     % back_time+1 as the propagation has been
                                     % performed 
-                                    Yhat = obj.setup.measure(x_propagate,obj.init.params,obj.setup.time(back_time));
+                                    Yhat = obj.setup.measure(x_propagate,obj.init.params,obj.setup.time(back_time),obj.init.input_story(traj).val(:,back_time),obj);
                                     % get filters - yhat
-                                    obj.init.Yhat_full_story(traj).val(1,:,back_time) = Yhat;            
+                                    obj.init.Yhat_full_story(traj).val(1,:,back_time) = Yhat;  
                                     tspan_pos = [max(1,back_time-1), back_time];
                                     [dyhat, x_filter] = obj.measure_filter(obj.init.Yhat_full_story(traj).val,tspan_pos,obj.init.X_filter_est(obj.init.traj));                                
                                     yhat(traj).val = Yhat;
                                     for filt=1:obj.setup.Nfilt
                                         for dim=1:obj.setup.dim_out
-                                            y_tmp = dyhat{filt,dim}.val(end);
-                                            obj.init.X_filter_est(traj).val{filt,dim}(:,tspan_pos) = x_filter{filt,dim}.val;
+                                            y_tmp(dim,1) = dyhat{filt,dim}.val(end);
+                                            obj.init.X_filter_est(traj).val{filt,dim}(:,unique(tspan_pos)) = x_filter{filt,dim}.val;
                                         end
-                                        yhat(traj).val = [yhat(traj).val, y_tmp]; 
-                                    end                                    
+                                        yhat(traj).val = [yhat(traj).val, y_tmp];
+                                    end
+
+
                                     for term=1:obj.setup.J_nterm
-                                        obj.init.Yhat_full_story(traj).val(term,:,back_time) = yhat(traj).val(:,term);                                        
+                                        obj.init.Yhat_full_story(traj).val(term,:,back_time) = yhat(traj).val(:,term);
                                     end
-                                end
-                                %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
-                                obj.init.Jstory(1,end+1) = J;
-                                if obj.setup.Jterm_store
-                                    obj.init.Jterm_story(:,end+1) = obj_tmp.init.Jterm_store;
-                                end                              
-                            end
-                        else
-                            % on each trajectory
-                            for traj=1:obj.setup.Ntraj
-                                obj.init.traj = traj;
-                                % keep the initial guess
-                                obj.init.X_est(traj).val(obj.setup.opt_vars,obj.init.BackIterIndex) = obj.init.temp_x0_opt;
-                            end
-                            
-                            % restore params
-                            obj.init.params = obj.setup.params.params_update(obj.init.params,obj.init.temp_x0(traj).val);
-                        end
+                                    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-                        % stop time counter
-                        obj.init.opt_time(end+1) = toc(opt_time);
+                                    %%%%%%%%%%% PROPAGATION %%%%%%%%%%%%%%%%%%%%%%%
+                                    n_iter_propagate = obj.init.ActualTimeIndex-back_time;
+                                    if ~strcmp(func2str(obj.setup.ode),'odeLsim')
+                                        
+                                        for j=1:n_iter_propagate
+                                            
+                                            % back time
+                                            back_time = obj.init.BackIterIndex+j;
+                                            tspan = obj.setup.time(back_time-1:back_time);                                            
+
+                                            % how do you handle the input?
+                                            obj.init.params.ActualTimeIndex = back_time; % here you have the -1 because BackIterIndex is differently set up than in measure_function                                                                         
+
+                                            % integrate
+                                            obj.init.t_ode_start = tic;                     
+                                            X = obj.setup.ode(@(t,x)obj.setup.model(t, x, obj.init.params, obj), tspan, x_propagate,obj.setup.odeset);                                    
+                                            x_propagate = X.y(:,end);                      
+                                            obj.init.X_est(traj).val(:,back_time) = x_propagate;
+                                            obj.init.t_ode(end+1) = toc(obj.init.t_ode_start);
+                                            
+                                        end
+                                        
+                                    else
+                                        
+                                        tspan = obj.setup.time(back_time:obj.init.ActualTimeIndex);
+                                        % integrate
+                                        obj.init.t_ode_start = tic;                     
+                                        X = obj.setup.ode(@(t,x)obj.setup.model(t, x, obj.init.params, obj), tspan, x_propagate,obj.setup.odeset);                                    
+                                        x_propagate = X.y(:,end);                      
+                                        obj.init.X_est(traj).val(:,back_time:obj.init.ActualTimeIndex) = X.y;
+                                        obj.init.t_ode(end+1) = toc(obj.init.t_ode_start);
+                                        
+                                    end
 
-                    end
+                                    for j =1:n_iter_propagate                                                                                
 
-                end
+                                        % back time
+                                        back_time = obj.init.BackIterIndex+j;                                        
 
-                if obj.setup.print
-                    clc;
-                end
+                                        % how do you handle the input?
+                                        obj.init.params.ActualTimeIndex = back_time; % here you have the -1 because BackIterIndex is differently set up than in measure_function                                          
+                                        
+                                        if strcmp(func2str(obj.setup.ode),'odeLsim')                                        
+                                            x_propagate = X.y(:,back_time);
+                                        else
+                                            x_propagate = obj.init.X_est(traj).val(:,back_time);
+                                        end
 
-            end
+                                        %%%% ESTIMATED measurements
+                                        % measures       
+                                        % NB: the output storage has to be done in
+                                        % back_time+1 as the propagation has been
+                                        % performed 
+                                        Yhat = obj.setup.measure(x_propagate,obj.init.params,obj.setup.time(back_time),obj.init.input_story(traj).val(:,back_time-1),obj);
+                                        % get filters - yhat
+                                        obj.init.Yhat_full_story(traj).val(1,:,back_time) = Yhat;            
+                                        tspan_pos = [max(1,back_time-1), back_time];
+                                        [dyhat, x_filter] = obj.measure_filter(obj.init.Yhat_full_story(traj).val,tspan_pos,obj.init.X_filter_est(obj.init.traj));                                
+                                        yhat(traj).val = Yhat;
+                                        for filt=1:obj.setup.Nfilt
+                                            for dim=1:obj.setup.dim_out
+                                                y_tmp(dim,1) = dyhat{filt,dim}.val(end);
+                                                obj.init.X_filter_est(traj).val{filt,dim}(:,unique(tspan_pos)) = x_filter{filt,dim}.val;
+                                            end
+                                            yhat(traj).val = [yhat(traj).val, y_tmp]; 
+                                        end                                    
+                                        for term=1:obj.setup.J_nterm
+                                            obj.init.Yhat_full_story(traj).val(term,:,back_time) = yhat(traj).val(:,term);                                        
+                                        end
+                                    end
+                                    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
+                                    obj.init.Jstory(1,end+1) = J;
+                                    if obj.setup.Jterm_store
+                                        obj.init.Jterm_story(:,end+1) = obj_tmp.init.Jterm_store;
+                                    end                              
+                                end
+                            else
+                                % on each trajectory
+                                for traj=1:obj.setup.Ntraj
+                                    obj.init.traj = traj;
+                                    % keep the initial guess
+                                    obj.init.X_est(traj).val(obj.setup.opt_vars,obj.init.BackIterIndex) = obj.init.temp_x0_opt;
+                                end
 
-            % on each trajectory
-            for traj=1:obj.setup.Ntraj
-                obj.init.traj = traj;
-                % save measures
-                obj.init.Y_est(traj).val = obj.init.X_est(traj).val(obj.setup.observed_state,:);
-                obj.init.Y_est_runtime(traj).val = obj.init.X_est_runtime(traj).val(obj.setup.observed_state,:);
-            end
-        end        
-        
-        % plot results for control design
-        function plot_section_control(obj,varargin)
-            
-            %%%% plot state estimation %%%
-            figure(1)
-            sgtitle('State estimation')
-            for i=1:obj.setup.plot_vars
-                subplot(obj.setup.plot_vars,1,i);
-                hold on
-                grid on
-                box on
-                
-                for traj=1:obj.setup.Ntraj
-                    if strcat(obj.setup.DataType,'simulated')
-                        plot(obj.setup.time,obj.init.X(traj).val(i,:),'b--');
-                    end
-                    plot(obj.setup.time,obj.init.X_est(traj).val(i,:),'r--');
-                    plot(obj.setup.time,obj.init.X_est_runtime(traj).val(i,:),'k:','LineWidth',0.5);                                        
+                                % restore params
+                                obj.init.params = obj.setup.params.params_update(obj.init.params,obj.init.temp_x0(traj).val);                                                        
+                            end
 
-                    if strcat(obj.setup.DataType,'simulated')
-                        legend('True','Est')
-%                         legend('Stored','Est','Runtime')
-                    else
-                        legend('Stored','Est','Runtime')
-                    end
-                end
-                
-                % labels
-                xlabel(['time [s]'])
-                ylabel(['x_',num2str(i)])
-            end
-            
-            %%%% plot state estimation error %%%
-            figure(2)
-            sgtitle('Estimation error - components')
-            
-            for i=1:obj.setup.plot_vars
-                subplot(obj.setup.plot_vars,1,i);
-                hold on
-                grid on
-                box on
-                
-                % plot
-                est_error = obj.init.X(1).val(i,:) - obj.init.X_est_runtime(1).val(i,:);
-                
-                log_flag = 1;
-                if ~log_flag
-                    plot(obj.setup.time,est_error,'k','LineWidth',2);
-                else
-                    % log 
-%                     set(gca, 'XScale', 'log')
-                    set(gca, 'YScale', 'log')
-                    plot(obj.setup.time,abs(est_error),'k','LineWidth',2);
-                end
-                
-                xlabel('time [s]')
-                ylabel(['\delta x_',num2str(i)])
-            end
-            
-            %%%% plot state estimation error - norm%%%
-            figure(3)
-            sgtitle('Estimation error - norm')
-            hold on
-            grid on
-            box on
+                            % stop time counter
+                            obj.init.opt_time(end+1) = toc(opt_time);
 
-            % plot
-            for iter=1:obj.setup.Niter
-                est_error_norm(iter) = norm(obj.init.X(1).val(1:obj.init.params.dim_state,iter) - obj.init.X_est_runtime(1).val(1:obj.init.params.dim_state,iter));
-            end
+                        end
+                    end
 
-            log_flag = 1;
-            if ~log_flag
-                plot(obj.setup.time,est_error_norm,'k','LineWidth',2);
-            else
-                % log 
-%                     set(gca, 'XScale', 'log')
-                set(gca, 'YScale', 'log')
-                plot(obj.setup.time,abs(est_error_norm),'k--','LineWidth',2);
-            end
+                else                    
 
-            xlabel('time [s]')
-            ylabel('\delta x_norm')            
-            
-            
-            %%%% plot windowed data %%%%
-            figure(4)
-            grid on
-            sgtitle('Sampled measured')
-            ax = zeros(1,3);
-            for k=1:obj.setup.dim_out
-                
-                % number fo subplots depending on the output dimension
-                n_subplot = obj.setup.dim_out;
-                
-                % indicize axes
-                ax_index = k;
-                ax(ax_index)=subplot(n_subplot,1,ax_index);
-                
-                % hold on on plots
-                hold on
-                
-                % dpwn sampling instants
-                WindowTime = obj.setup.time(obj.init.temp_time);
-                
-                for traj=1:obj.setup.Ntraj
-                    % plot true values
-                    y_meas = reshape(obj.init.Y_full_story(traj).val(1,k,:),size(obj.setup.time));
-                    plot(obj.setup.time,y_meas,'m--')
-                    
-                    % plot down sampling
-                    data = reshape(obj.init.Yhat_full_story(traj).val(1,k,obj.init.temp_time),1,length(WindowTime));
-%                     plot(WindowTime,data,'s','MarkerSize',5);
-
-                    % plot target values
-                    data = reshape(obj.init.target_story(traj).val(1,k,obj.init.temp_time),1,length(WindowTime));
-                    plot(WindowTime,data,'bo','MarkerSize',5);
-
-                    ylabel(strcat('y_',num2str(k)));
-                    xlabel('simulation time [s]');
-%                     legend('true','estimation','target')
-                    legend('trajectory','measures')
                 end
-            end
-            linkaxes(ax,'x');
-            
-            %%%% plot filters %%%%%
-            figure(5)
-            sgtitle('Filters on measures')            
-            ax = zeros(1,3);
-            for k=1:obj.setup.J_nterm
-                
-                % number fo subplots depending on the Nterm
-                n_subplot = obj.setup.J_nterm;
-                
-                % indicize axes
-                ax_index = k;
-                ax(ax_index)=subplot(n_subplot,1,ax_index);                
-                
-                % plot
-                hold on
-                grid on
-                
-                for traj=1:obj.setup.Ntraj
-                    for dim=1:obj.setup.dim_out
-                        y_plot = obj.setup.J_temp_scale(k)*reshape(obj.init.Y_full_story(traj).val(k,dim,:),size(obj.setup.time));
-                        yhat_plot = obj.setup.J_temp_scale(k)*reshape(obj.init.Yhat_full_story(traj).val(k,dim,:),size(obj.setup.time));
-                        plot(obj.setup.time,y_plot,'b--');
-                        plot(obj.setup.time,yhat_plot,'r--');
-                    end
-                    
-                    legend('measured','estimated')
-                    ylabel(strcat('y_{filter}^',num2str(k)));
-                    xlabel('simulation time [s]');
-                end            
-                
-            end
-            linkaxes(ax,'x');
-            
+
+                if obj.setup.print
+                    clc;
+                end                
+            else
+            end            
         end
     end
 end
diff --git a/Lib/obs/obsopt_new.m b/Lib/obs/obsopt_new.m
new file mode 100644
index 0000000..118727a
--- /dev/null
+++ b/Lib/obs/obsopt_new.m
@@ -0,0 +1,1763 @@
+%%%%%%% LICENSING %%%%%%%
+% Copyright 2020-2021 Federico Oliva
+% This program is free software: you can redistribute it and/or modify
+% it under the terms of the GNU General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+
+% This program is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU General Public License for more details.
+
+% You should have received a copy of the GNU General Public License
+% along with this program.  If not, see <http://www.gnu.org/licenses/>.
+%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%%%%%%% CODE %%%%%%%%%%%%
+
+%% class definition - obsopt observer
+% This class allows to apply a newton-like observer to a specified model,
+% within a model integration setup in the form:
+% 1) initial setup
+% ---- for loop ----
+% 2) model integration 
+% 3) measurements (with noise)
+% 4) newton-like observer (obsopt)
+% -----> back to 2
+% 5) plot section (optional)
+classdef obsopt_new < handle
+    %%% class properties
+    properties
+        % setup: set of flags defining the main characteristics. These
+        % flags can be set by properly calling the class constructor.
+        setup;
+        
+        % init: set of fags initialised depending on setup and set of
+        % variables that will be used and stored during the observation
+        % process. 
+        init;
+    end
+    
+    %%% class methods %%%
+    methods
+        % class constructor. Each of these variables can be specifically
+        % set by calling the constructor with specific properties, i.e. by
+        % properly using the varargin parameter. Otherwise a default system
+        % is set up
+        function obj = obsopt_new(varargin)
+            
+            if any(strcmp(varargin,'params'))
+                pos = find(strcmp(varargin,'params'));
+                params = varargin{pos+1};
+                
+                % get model from params
+                obj.setup.model = params.model;
+                
+                % get measure from params
+                obj.setup.measure = params.measure;
+                
+                % get the integration algorithm
+                obj.setup.ode = params.ode;
+                obj.setup.odeset = params.odeset;
+                
+                % get the samling time from params
+                obj.setup.Ts = params.Ts;
+                
+                % get the time instants vector
+                obj.setup.time = params.time;
+                
+                % output dimension
+                obj.setup.dim_out = params.OutDim;
+                obj.setup.dim_out_compare = params.OutDim_compare;
+            
+                % state dimension
+                obj.setup.dim_state = params.StateDim;
+                
+                % noise info
+                obj.setup.noise = params.noise;
+                obj.setup.noise_mu = params.noise_mu;
+                obj.setup.noise_std = params.noise_std;
+                
+                % observed state
+                obj.setup.observed_state = params.observed_state;
+                
+                % get state init
+                obj.setup.X = params.X;
+                obj.setup.X_est = params.X_est;
+                
+                % estimated param
+                obj.setup.estimated_params = params.estimated_params;
+                obj.setup.opt_vars = params.opt_vars;
+                obj.setup.nonopt_vars = params.nonopt_vars;
+                obj.setup.plot_vars = params.plot_vars;
+                obj.setup.plot_params = params.plot_params;
+                                
+            else
+                obj.setup.model = @(t,x,params) -2*x;
+                obj.setup.measure = @(x,params) x;
+                obj.setup.ode = @ode45;
+                obj.setup.Ts = 1e-1;
+                
+                % get the time instants vector. Default is from 0 to 1s, with
+                % the sample time previously set.
+                obj.setup.time = 0:obj.setup.Ts:10;
+                
+                obj.setup.dim_out = 1;
+                obj.setup.dim_state = 1;
+                
+                % set empty params
+                obj.setup.X = 5;
+                
+                % noise info
+                obj.setup.noise = 1;
+                obj.setup.noise_mu = 0;
+                obj.setup.noise_std = 1e-1;
+                
+                % observed state
+                obj.setup.observed_state = 1;
+                
+                % input
+                params.input = @(t,x,params) 0;
+                
+                % estimated param
+                obj.setup.estimated_params = [];
+            end
+            
+            % get number of iterations
+            obj.setup.Niter = length(obj.setup.time);
+            
+            % define the time span from the sample time. This will be used
+            % in the integration procedure (e.g. ode45)
+            obj.setup.tspan = [0, obj.setup.Ts];
+            
+            % get if data are simulated or from real measurements (used in)
+            % plot section
+            if any(strcmp(varargin,'DataType'))
+                pos = find(strcmp(varargin,'DataType'));
+                obj.setup.DataType = varargin{pos+1};
+            else
+                obj.setup.DataType = 'simulated';
+            end
+            
+            % run or no the optimisation
+            if any(strcmp(varargin,'optimise'))
+                pos = find(strcmp(varargin,'optimise'));
+                obj.setup.optimise = varargin{pos+1};
+            else
+                obj.setup.optimise = 1;
+            end
+                       
+            % option to print out the optimisation process or not. Default
+            % is not (0).
+            if any(strcmp(varargin,'print'))
+                pos = find(strcmp(varargin,'print'));
+                obj.setup.print = varargin{pos+1};
+            else
+                obj.setup.print = 0;
+            end
+            
+            % option to define the safety interval in adaptive sampling
+            if any(strcmp(varargin,'SafetyDensity'))
+                pos = find(strcmp(varargin,'SafetyDensity'));
+                obj.setup.safety_density = varargin{pos+1};
+            else
+                obj.setup.safety_density = 2;
+            end
+            
+            % enable or not the adaptive sampling
+            if any(strcmp(varargin,'AdaptiveSampling'))
+                pos = find(strcmp(varargin,'AdaptiveSampling'));
+                obj.setup.AdaptiveSampling = varargin{pos+1};
+            else
+                obj.setup.AdaptiveSampling = 0;
+            end
+            obj.init.nfreqs = 2;            
+            obj.init.freqs = zeros(obj.init.nfreqs,1);
+            obj.init.wvname = 'amor';
+            obj.init.Nv = 48;
+            obj.init.PLIMITS = [1e-2*obj.setup.Ts 2e2*obj.setup.Ts];
+            obj.init.FLIMITS = fliplr(1./obj.init.PLIMITS);            
+            obj.init.NtsChanged = 0;
+            obj.init.break = 0;
+                        
+            
+            % option to define the safety interval in adaptive sampling. If
+            % AdaptiveSampling flag is zero the histeresis is set to zero,
+            % disabling the adaptive sampling
+            if any(strcmp(varargin,'AdaptiveParams')) && (obj.setup.AdaptiveSampling)
+                pos = find(strcmp(varargin,'AdaptiveParams'));
+                tmp = varargin{pos+1};                         
+                obj.init.Fnyq = tmp(1);     % integer
+                obj.init.Fbuflen = tmp(2);  % integer < Nw
+                obj.init.Fselect = tmp(3);  % 1 = min 2 = max 
+                obj.init.FNts = tmp(4);
+                obj.init.Fmin = tmp(5);
+                obj.init.epsD = tmp(6);
+                obj.init.PE_flag = tmp(7);
+                obj.init.wavelet_output_dim = tmp(8:end);
+            else
+                obj.init.FNts = 1;
+                obj.init.Fbuflen = 20;
+                obj.init.Fselect = 2;
+                obj.init.Fnyq = 2;
+                obj.init.FNts = 0;
+                obj.init.PE_flag = 1;
+                obj.init.epsD = Inf;
+                obj.init.wavelet_output_dim = [];
+            end
+            
+            % enable or not the buffer flush on the adaptive sampling
+            if any(strcmp(varargin,'FlushBuffer')) && (obj.setup.AdaptiveSampling)
+                pos = find(strcmp(varargin,'FlushBuffer'));
+                obj.setup.FlushBuffer = (obj.setup.AdaptiveSampling && varargin{pos+1});
+            else
+                obj.setup.FlushBuffer = 0;
+            end
+            
+            % get the maximum number of iterations in the optimisation
+            % process. Default is 100.
+            if any(strcmp(varargin,'MaxIter'))
+                pos = find(strcmp(varargin,'MaxIter'));
+                obj.setup.max_iterVal = varargin{pos+1};
+                if length(obj.setup.max_iterVal) == 1
+                    obj.setup.max_iter = obj.setup.max_iterVal;                    
+                else
+                    obj.setup.max_iter = max(obj.setup.max_iterVal);
+                end
+            else
+                obj.setup.max_iter = 100;
+            end
+            obj.init.MaxIter_story = [];
+            
+            % get the maximum number of iterations in the optimisation
+            % process. Default is 100.
+            if any(strcmp(varargin,'MaxOptTime'))
+                pos = find(strcmp(varargin,'MaxOptTime'));
+                obj.setup.MaxOptTime = varargin{pos+1};
+                obj.setup.MaxOptTimeFlag = 0;
+            else
+                obj.setup.MaxOptTime = Inf;
+                obj.setup.MaxOptTimeFlag = 0;
+            end
+            % init story
+            obj.init.MaxOptTime_story = [];
+            
+            % check if the optimised value shall always be accepted. This
+            % goes in contrast with the role of Jdot_thresh
+            if any(strcmp(varargin,'AlwaysOpt'))
+                pos = find(strcmp(varargin,'AlwaysOpt'));
+                obj.setup.AlwaysOpt = varargin{pos+1};
+            else
+                obj.setup.AlwaysOpt = 1;
+            end
+
+            % get the Nw buffer length. See reference on the algorithm for
+            % more detailed information. Default is 5.
+            if any(strcmp(varargin,'Nw'))
+                pos = find(strcmp(varargin,'Nw'));
+                obj.setup.w = varargin{pos+1};
+            else
+                obj.setup.w = 5;
+            end
+            
+            % get the Nts down sampling period. See reference on the algorithm for
+            % more detailed information. Default is 3.
+            if any(strcmp(varargin,'Nts'))
+                pos = find(strcmp(varargin,'Nts'));
+                obj.setup.NtsVal = varargin{pos+1};
+                if length(obj.setup.NtsVal) == 1
+                    obj.setup.Nts = obj.setup.NtsVal;
+                    obj.setup.NtsVal = ones(1,obj.setup.w)*obj.setup.Nts;
+                else
+                    obj.setup.Nts = min(obj.setup.NtsVal);
+                end
+            else
+                obj.setup.Nts = 3;
+            end
+            obj.init.Nsaved = 0;
+            
+            % Discrete sampling time (for filters)
+            obj.setup.DTs = obj.setup.Nts*obj.setup.Ts;
+
+            % get the conditions to consider the optimisation result. See 
+            % reference on the algorithm for more detailed information. 
+            % Default is 90%.
+            if any(strcmp(varargin,'Jdot_thresh'))
+                pos = find(strcmp(varargin,'Jdot_thresh'));
+                obj.setup.Jdot_thresh = varargin{pos+1};
+            else
+                obj.setup.Jdot_thresh = 0.9;
+            end
+
+            % get the conditions for the optimisation to stop. Default are
+            % 1e-10 and 1e3. Check out outfun options for optimisation
+            % methods like fminsearch or fminunc in MATLAB
+            if any(strcmp(varargin,'J_thresh'))
+                pos = find(strcmp(varargin,'J_thresh'));
+                obj.setup.J_thresh = varargin{pos+1};
+            else
+                obj.setup.J_thresh = [1e-10, 1e3];
+            end
+            
+            % normalise the cost function
+            if any(strcmp(varargin,'J_normalise'))
+                pos = find(strcmp(varargin,'J_normalise'));
+                obj.setup.J_normalise = varargin{pos+1};
+            else
+                obj.setup.J_normalise = 1;
+            end
+            
+            % change max iterations depending on PE 
+            if any(strcmp(varargin,'PE_maxiter'))
+                pos = find(strcmp(varargin,'PE_maxiter'));
+                obj.setup.PE_maxiter = varargin{pos+1};
+            else
+                obj.setup.PE_maxiter = 0;
+            end
+            
+            % PE position
+            if any(strcmp(varargin,'PEPos'))
+                pos = find(strcmp(varargin,'PEPos'));
+                obj.setup.PEPos = varargin{pos+1};
+            else
+                obj.setup.PEPos = [1 1];
+            end
+
+            % get the optimisation method. Default is fminsearch from
+            % MATLAB
+            if any(strcmp(varargin,'opt'))
+                pos = find(strcmp(varargin,'opt'));
+                obj.setup.fmin = varargin{pos+1};
+            else
+                obj.setup.fmin = @fminsearch;
+            end                        
+            
+            % get the multistart option
+            if any(strcmp(varargin,'MultiStart'))
+                pos = find(strcmp(varargin,'MultiStart'));
+                obj.setup.MultiStart = varargin{pos+1};
+            else
+                obj.setup.MultiStart = 0;
+            end
+            
+            % get the multistart option
+            test = func2str(obj.setup.fmin);
+            if any(strcmp(varargin,'Bounds')) && ~strcmp(test,'fmincon')
+                pos = find(strcmp(varargin,'Bounds'));
+                obj.setup.bounds = varargin{pos+1};                
+            else
+                obj.setup.bounds = 0;
+            end
+            
+            % get the multistart option
+            if any(strcmp(varargin,'BoundsPos'))
+                pos = find(strcmp(varargin,'BoundsPos'));
+                obj.setup.boundsPos = varargin{pos+1};                
+            else
+                obj.setup.boundsPos = [];
+            end            
+            
+            % get the multistart option
+            if any(strcmp(varargin,'BoundsValLow'))
+                pos = find(strcmp(varargin,'BoundsValLow'));
+                obj.setup.boundsValLow = varargin{pos+1};                
+            else
+                obj.setup.boundsValLow = [];
+            end
+            
+            % get the multistart option
+            if any(strcmp(varargin,'BoundsValUp'))
+                pos = find(strcmp(varargin,'BoundsValUp'));
+                obj.setup.boundsValUp = varargin{pos+1};                
+            else
+                obj.setup.boundsValUp = [];
+            end
+            
+            % get the multistart option
+            if any(strcmp(varargin,'BoundsWeight'))
+                pos = find(strcmp(varargin,'BoundsWeight'));
+                obj.setup.boundsWeight = varargin{pos+1};                
+            else
+                obj.setup.boundsWeight = ones(1,numel(obj.setup.boundsPos));
+            end
+            
+            % constraint pos
+            if any(strcmp(varargin,'ConPos'))
+                pos = find(strcmp(varargin,'ConPos'));
+                obj.setup.ConPos = varargin{pos+1};
+            else
+                obj.setup.ConPos = [];
+            end
+            
+            
+            % handle fmincon
+            try
+                test = func2str(obj.setup.fmin);
+            catch
+                test = 'null';
+            end
+            if strcmp(test,'fmincon') || strcmp(test,'fminsearchbnd') || any(strcmp(varargin,'Bounds'))
+                if any(strcmp(varargin,'Acon'))
+                    pos = find(strcmp(varargin,'Acon'));
+                    obj.setup.Acon = varargin{pos+1};
+                else
+                    obj.setup.Acon = [];                    
+                end
+                if any(strcmp(varargin,'Bcon'))
+                    pos = find(strcmp(varargin,'Bcon'));
+                    obj.setup.Bcon = varargin{pos+1};
+                else
+                    obj.setup.Bcon = [];                
+                end
+                if any(strcmp(varargin,'Acon_eq'))
+                    pos = find(strcmp(varargin,'Acon_eq'));
+                    obj.setup.Acon_eq = varargin{pos+1};
+                else
+                    obj.setup.Acon_eq = [];                
+                end
+                if any(strcmp(varargin,'Bcon_eq'))
+                    pos = find(strcmp(varargin,'Bcon_eq'));
+                    obj.setup.Bcon_eq = varargin{pos+1};
+                else
+                    obj.setup.Bcon_eq = [];                
+                end
+                if any(strcmp(varargin,'LBcon'))
+                    obj.setup.LBcon = double(-Inf*ones(1,length(obj.setup.opt_vars)));
+                    pos = find(strcmp(varargin,'LBcon'));
+                    obj.setup.LBcon(obj.setup.ConPos) = varargin{pos+1};
+                else
+                    obj.setup.LBcon = zeros(1,length(obj.setup.opt_vars));                
+                end
+                if any(strcmp(varargin,'UBcon'))
+                    obj.setup.UBcon = double(Inf*ones(1,length(obj.setup.opt_vars)));
+                    pos = find(strcmp(varargin,'UBcon'));
+                    obj.setup.UBcon(obj.setup.ConPos) = varargin{pos+1};
+                else
+                    obj.setup.UBcon = zeros(1,length(obj.setup.opt_vars));          
+                end
+                if any(strcmp(varargin,'NONCOLcon'))
+                    pos = find(strcmp(varargin,'NONCOLcon'));
+                    obj.setup.NONCOLcon = varargin{pos+1}; 
+                else
+		    obj.setup.NONCOLcon = @obj.NONCOLcon;
+                end
+            else
+                obj.setup.Acon = [];    
+                obj.setup.Bcon = [];
+                obj.setup.Acon_eq = [];
+                obj.setup.Bcon_eq = [];
+                obj.setup.LBcon = [];
+                obj.setup.UBcon = [];
+                obj.setup.NONCOLcon = [];
+            end
+            
+            % store J terms
+            if any(strcmp(varargin,'Jterm_store')) && (~strcmp(obj.setup.fmin,'broyden'))
+                pos = find(strcmp(varargin,'Jterm_store'));
+                obj.setup.Jterm_store = varargin{pos+1};
+            else
+                if (~strcmp(obj.setup.fmin,'broyden'))
+                    obj.setup.Jterm_store = 1;
+                else
+                    obj.setup.Jterm_store = 0;
+                end
+            end
+
+            % get the algorithm direction (forward or backward). V1.1 has
+            % only forward implemented
+            if any(strcmp(varargin,'forward'))
+                pos = find(strcmp(varargin,'forward'));
+                obj.setup.forward = varargin{pos+1};
+            else
+                obj.setup.forward = 1;
+            end            
+            
+            % wait all buffer to be filled
+            if any(strcmp(varargin,'WaitAllBuffer'))
+                pos = find(strcmp(varargin,'WaitAllBuffer'));
+                obj.setup.WaitAllBuffer = varargin{pos+1};
+            else
+                obj.setup.WaitAllBuffer = 0;
+            end
+            
+            % reference model
+            if any(strcmp(varargin,'model_reference'))
+                pos = find(strcmp(varargin,'model_reference'));
+                obj.setup.model_reference = varargin{pos+1};
+            else
+                obj.setup.model_reference = obj.setup.model;
+            end
+            
+            % reference measure
+            if any(strcmp(varargin,'measure_reference'))
+                pos = find(strcmp(varargin,'measure_reference'));
+                obj.setup.measure_reference = varargin{pos+1};
+            else
+                obj.setup.measure_reference = obj.setup.measure;
+            end
+            
+            
+            % get the cost function terms. Default uses measures only, 
+            % without any additional filter (e.g. [1 0 0])
+            if any(strcmp(varargin,'filters'))
+                pos = find(strcmp(varargin,'filters'));
+                temp_scale = varargin{pos+1};
+            else
+                temp_scale = [1];
+            end
+            
+            
+            % filters
+            obj.setup.J_temp_scale = temp_scale;
+            obj.setup.J_nterm = length(temp_scale);
+            obj.setup.J_nterm_total = length(temp_scale);             
+            
+            % get the terminal like term in the cost function
+            if any(strcmp(varargin,'terminal'))
+                pos = find(strcmp(varargin,'terminal'));
+                if varargin{pos+1} ~= 0
+                    obj.setup.J_term_terminal = 1;
+                    obj.setup.J_temp_scale = [obj.setup.J_temp_scale, varargin{pos+1}];
+                    obj.setup.J_term_terminal_position = length(obj.setup.J_temp_scale);
+                    obj.setup.J_nterm_total = obj.setup.J_nterm_total+1;
+                else
+                    obj.setup.J_term_terminal = 0;
+                end
+            else
+                obj.setup.J_term_terminal = 0;
+            end     
+            
+            if any(strcmp(varargin,'terminal_states')) && obj.setup.J_term_terminal
+                pos = find(strcmp(varargin,'terminal_states'));
+                obj.setup.terminal_states = varargin{pos+1};
+            else
+                obj.setup.terminal_states = [];
+            end
+            
+            if any(strcmp(varargin,'terminal_weights')) && obj.setup.J_term_terminal
+                pos = find(strcmp(varargin,'terminal_weights'));
+                obj.setup.terminal_weights = varargin{pos+1};
+            else
+                obj.setup.terminal_weights = ones(size(obj.setup.terminal_states));
+            end
+            
+            if any(strcmp(varargin,'terminal_normalise')) && obj.setup.J_term_terminal
+                pos = find(strcmp(varargin,'terminal_normalise'));
+                obj.setup.terminal_normalise = varargin{pos+1};
+            else
+                obj.setup.terminal_normalise = 0;
+            end
+            
+            obj.setup.Nfilt = obj.setup.J_nterm-1;
+            
+            % option to define the safety interval in adaptive sampling
+            if any(strcmp(varargin,'filterTF'))
+                pos = find(strcmp(varargin,'filterTF'));
+                obj.setup.filterTF = varargin{pos+1};
+            else
+                obj.setup.filterTF = [];
+            end            
+            
+            % set of the integral resets or not
+            if any(strcmp(varargin,'int_reset'))
+                pos = find(strcmp(varargin,'int_reset'));
+                obj.setup.J_int_reset = varargin{pos+1};
+            else
+                obj.setup.J_int_reset = 0;
+            end
+            
+            if ~any(strcmp(varargin,'params'))
+                % set initial condition perturbed
+                obj.setup.X_est(:,1) = obj.setup.X(:,1) + 1e1*obj.setup.noise*(obj.setup.noise_mu + obj.setup.noise_std.*randn(obj.setup.dim_state,1));
+                
+                obj.setup.Ntraj = 1;
+            end
+            % complete the params update in .setup
+            obj.setup.params = params;
+            
+            % number of reference trajectories            
+            obj.setup.Ntraj = params.Ntraj;
+            
+            % set the actual cost function (J)
+            obj.setup.cost_run = @obj.cost_function;
+            
+            % initialise the class
+            obj = obj.obs_init();
+        end
+        
+        % init function. This function defines all the intenal variables
+        % that will be used during the optimisation process. Observed state
+        % trajectories will also be stored in these vars. 
+        function obj = obs_init(obj)
+            
+            % get params structure. It's redundant but by doing so we save
+            % the initial values of the params
+            obj.init.params = obj.setup.params;
+            obj.init.params = obj.setup.params.params_update(obj.init.params,obj.setup.X_est(1).val(:,1));
+            
+            % get initial state
+            obj.init.X = obj.setup.X;
+            obj.init.X_est = obj.setup.X_est;
+            obj.init.X_est_runtime = obj.setup.X_est;
+            obj.init.X_wrong = obj.setup.X_est;            
+            for traj=1:obj.setup.Ntraj
+                obj.init.X_filter(traj).val = cell(obj.setup.Nfilt,1);
+                obj.init.X_filter_est(traj).val = cell(obj.setup.Nfilt,1);
+                obj.init.X_filter_buffer_control(traj).val = cell(obj.setup.Nfilt,1);
+                for i=1:obj.setup.Nfilt
+                    for dim=1:obj.setup.dim_out
+                        obj.init.X_filter(traj).val{i,dim} = zeros(obj.setup.filterTF(i).dim,obj.setup.Niter);
+                        obj.init.X_filter_est(traj).val{i,dim} = 0*randn(obj.setup.filterTF(i).dim,obj.setup.Niter);
+                        obj.init.X_filter_buffer_control(traj).val{i,dim} = zeros(obj.setup.filterTF(i).dim,obj.setup.Niter);
+                    end
+                end
+            end
+            
+            % create filter matrix string
+            Nfilt = length(obj.setup.filterTF);
+            tmp_str = [];
+            for filt=1:Nfilt
+                tmp_str = [tmp_str,'obj.setup.filterTF(',num2str(filt),').A,obj.setup.filterTF(',num2str(filt),...
+                          ').B,obj.setup.filterTF(',num2str(filt),').C,obj.setup.filterTF(',num2str(filt),').D,'];
+            end
+            tmp_str = tmp_str(1:end-1);
+            if isempty(tmp_str)
+                obj.init.matrix_str = '0';
+            else
+                obj.init.matrix_str = tmp_str;
+            end
+            
+            % create scale factor, namely the weight over time for all the
+            % cost function terms. In V1.1 no forgetting factor is
+            % implemented. 
+            obj = obj.scale_factor();
+            obj.init.normalised = 0;
+            obj.init.ConstrNormalised = 0;
+            
+            % Window Samples: maximum number of time instants considered in
+            % the optimisation if fixed sampling is used. For more
+            % information check the reference. 
+            obj.init.WindowSamples = max(2,obj.setup.Nts*(obj.setup.w-1)+1);
+            
+            % get safety interval for the adaptive sampling
+            obj.init.safety_interval = int32(obj.setup.safety_density*obj.init.WindowSamples);
+            
+            % BackIterIndex: t0 index (starting from 1). For more
+            % information check the reference. 
+            obj.init.BackIterIndex = 1;            
+            
+            % persistenct excitation setup
+            obj.init.c1_PE = 5;
+            obj.init.d1_PE = 20;
+            obj.init.PE_pos_array = [];
+            for i=1:obj.setup.Ntraj
+                obj.init.buf_PE(i).val = zeros(1,obj.init.d1_PE);
+            end
+
+            % measure buffer: these buffers are used to store the measured
+            % and estimated values on the observed states.           
+            for i=1:obj.setup.Ntraj
+                obj.init.Y(i).val =  zeros(obj.setup.J_nterm,obj.setup.dim_out,obj.setup.w);
+                obj.init.Ytrue_full_story(i).val = zeros(obj.setup.J_nterm,obj.setup.dim_out,0);
+                obj.init.Y_full_story(i).val = zeros(obj.setup.J_nterm,obj.setup.dim_out,0);
+                obj.init.Yhat_full_story(i).val = zeros(obj.setup.J_nterm,obj.setup.dim_out,0);
+                obj.init.target_story(i).val = zeros(obj.setup.J_nterm,obj.setup.dim_out,0);
+                % buffer for Y during drive computation (control design)
+                obj.init.Y_buffer_control(i).val = [];
+                obj.init.drive_out(i).val = [];
+                obj.init.input_story(i).val(:,1) = zeros(obj.setup.params.dim_input,1);
+                obj.init.input_story_ref(i).val(:,1) = zeros(obj.setup.params.dim_input,1);
+            end
+            
+            % input dimension
+            obj.setup.dim_input = obj.setup.params.dim_input;
+
+            % buffer adaptive sampling: these buffers keep track of the
+            % time instants in which the measured data have been stored. 
+            obj.init.Y_space = zeros(1,obj.setup.w);
+            obj.init.Y_space_full_story = 0;            
+
+            % dJ condition buffer (adaptive sampling): numerical condition
+            % to be thresholded in order to trigger the adaptive sampling.
+            % Not implemented in V1.1
+            obj.init.dJ_cond_story = [];
+            
+            % observer cost function init
+            obj.init.J = 0;
+            obj.init.Jstory = obj.init.J;
+            obj.init.Jterm_store = zeros(obj.setup.J_nterm_total,1);
+            obj.init.Jterm_story = obj.init.Jterm_store;
+            obj.init.Jdot_story = 0;
+            % J_components is used to keep track of the different cost
+            % function terms amplitude. Not implemented in V1.1
+            obj.init.J_components = ones(obj.setup.dim_out,obj.setup.J_nterm);
+            obj.init.just_optimised = 0;
+            obj.init.FirstOpt = 0;
+            
+            % time instants in which the optimisation is run
+            obj.init.temp_time = [];
+            % time instand in which the optimisation is accepted 
+            % (see J_dot_thresh for more information)
+            obj.init.opt_chosen_time = [];
+            
+            % single integration time
+            obj.init.t_ode = 0;
+            obj.init.t_meas = 0;
+            obj.init.t_filt = 0;
+            obj.init.t_J = 0;
+            obj.init.t_ode_start = 0;
+            obj.init.t_meas_start = 0;
+            obj.init.t_filt_start = 0;
+            obj.init.t_J_start = 0;
+            
+            % cost function gradient memory buffer. Not implemented in V1.1
+            obj.init.grad_story = zeros(obj.setup.dim_state,1);
+            
+            % optimisation counters. Related to temp_time and chosen_time
+            obj.init.opt_counter = 0;
+            obj.init.select_counter = 0;
+            
+            %%% start of optimisation setup %%%
+            % optimset: check documentation for fminsearch or fminunc
+            obj.init.TolX = 0;
+            obj.init.TolFun = 0;
+            obj.init.DiffMinChange = 1e-3;
+            obj.init.last_opt_time = 0;
+            obj.init.opt_time = 0;
+            
+            % set options
+            if obj.setup.print 
+                obj.init.display = 'iter';
+            else
+                obj.init.display = 'off'; 
+            end
+    
+            
+            % optimset                    
+            if strcmp(func2str(obj.setup.fmin),'fminsearchcon')
+                obj.init.myoptioptions = optimset('MaxIter', obj.setup.max_iter, 'display',obj.init.display, ...
+                                                  'MaxFunEvals',Inf,'OutputFcn',@obj.outfun,'TolFun',obj.init.TolFun,'TolX',obj.init.TolX);   
+            elseif strcmp(func2str(obj.setup.fmin),'patternsearch')                              
+                obj.init.myoptioptions = optimoptions(func2str(obj.setup.fmin), 'MaxIter', obj.setup.max_iter, 'display',obj.init.display, ...
+                                                      'Cache', 'on', 'UseParallel', false, 'StepTolerance', 0,'MaxFunEvals',Inf,'Algorithm','nups');            
+            else
+                obj.init.myoptioptions = optimoptions(func2str(obj.setup.fmin), 'MaxIter', obj.setup.max_iter, 'display',obj.init.display, ...
+                                                      'OptimalityTolerance', 0, 'StepTolerance', 0,'MaxFunEvals',Inf, 'GradObj', 'off',...
+                                                      'OutputFcn',@obj.outfun,'TolFun',obj.init.TolFun,'TolX',obj.init.TolX, ...
+                                                      'FiniteDifferenceStepSize', obj.init.DiffMinChange, 'FiniteDifferenceType','central');                          
+            end
+            %%% end of optimisation setup %%%
+
+        end
+
+        % nonlinear constraing default
+        function [c, ceq] = NONCOLcon(varargin)
+            c = 0;
+            ceq = 0;
+        end
+        
+        % scale factor: this method defines the cost function weights
+        % accordingly to the selected filters (see setup.temp_scale).
+        function obj = scale_factor(obj)
+            for dim=1:obj.setup.dim_out
+                obj.init.scale_factor(dim,:) = obj.setup.J_temp_scale.*ones(1,obj.setup.J_nterm_total);
+            end
+        end
+        
+        % outfun: this method check wether or not the optimisation process
+        % shall be stopped or not. Check setup.J_thresh for more
+        % information. 
+        function stop = outfun(obj,x, optimValues, state)
+            if (optimValues.iteration == obj.setup.max_iter) || (optimValues.fval <= obj.init.TolFun) || obj.setup.MaxOptTimeFlag
+                stop = true;
+            else
+                stop = false;
+            end
+        end
+        
+        % convex conjugate: this function computes the convex conjugate of
+        % the cost function
+        function [J_final,x_final,obj] = convex_conjugate(obj,varargin)
+            
+            % get opt state
+            x_opt = varargin{1};           
+            
+            % define the function handle for the conjugate
+            y = ones(size(x_opt));
+            f_cc = @(x)-(transpose(y)*x - obj.cost_function(x,varargin{2},varargin{3},varargin{4}));
+            
+            % initial condition
+            x_0 = x_opt;
+            
+            % solve the sup problem to find the conjugate fcn
+            [x_final,J_final] = fminunc(f_cc,x_0,obj.init.myoptioptions);
+            
+            obj.init.convex_conjugate(end+1) = J_final;
+            
+        end                
+        
+        % cost function: objective to be minimised by the MHE observer
+        function [J_final,obj] = cost_function(obj,varargin) 
+            
+            obj.init.t_J_start = tic;
+            
+            % above ntraj init
+            J_final = 0;
+            J_input = 0;
+                
+            for traj = 1:obj.setup.Ntraj
+                
+                obj.init.params.optimising = 1;
+                
+                obj.init.traj = traj;
+                obj.init.params.traj = traj;
+                
+                % cost function init
+                Jtot = 0;
+                
+                % get opt state
+                x_opt = varargin{1};
+
+                % non optimised vals
+                x_nonopt = varargin{2}(traj).val;
+                
+                % x filter vals
+                x_filters = varargin{3}(traj).val;
+                Lfilt = length(x_filters);
+
+                % create state
+                x = zeros(obj.setup.dim_state+Lfilt,1);
+                x(obj.setup.opt_vars) = x_opt;
+                x(obj.setup.nonopt_vars) = x_nonopt;
+                x(obj.setup.dim_state+1:end) = x_filters;
+                
+                % update params
+                obj.init.params = obj.setup.params.params_update(obj.init.params,x);
+                
+                % get desired trajectory
+                y_target = varargin{4}(traj).val;
+
+                % init Jterm store
+                obj.init.Jterm_store = zeros(obj.setup.J_nterm_total,1);
+                
+                %%% integrate %%%
+                % define time array
+                tspan_pos = [obj.init.BackIterIndex, nonzeros(obj.init.Y_space)'];
+                tspan = obj.setup.time(tspan_pos(1):tspan_pos(end));  
+                buf_dist = diff(tspan_pos);
+                n_iter = sum(buf_dist);
+                % initial condition
+                x_start = x(1:obj.setup.dim_state);      
+                % reset buffer for Y during drive computation (control design)
+                obj.init.Y_buffer_control(traj).val = [];
+                for i=1:obj.setup.Nfilt
+                    for dim=1:obj.setup.dim_out
+                        obj.init.X_filter_buffer_control(traj).val{i,dim} = 0*obj.init.X_filter_buffer_control(traj).val{i,dim};
+                    end
+                end
+                % get evolution with input only if at least 2 iinstans are
+                % considered
+                if length(tspan)>1
+                    X = obj.setup.ode(@(t,x)obj.setup.model(t, x, obj.init.params, obj), tspan , x_start, obj.setup.odeset);
+                else
+                    X.y = x_start;
+                end
+                
+                obj.init.params.optimising = 0;
+                
+                % check for NaN or Inf
+                NaN_Flag = find(isnan(X.y));
+                if NaN_Flag
+                    J_final = NaN;
+                    break
+                end
+                
+                Inf_Flag = isempty(isinf(X.y));
+                Huge_Flag = isempty(X.y>1e10);
+                if Inf_Flag || Huge_Flag
+                    J_final = Inf;
+                    break
+                end                
+                
+                %%% get measure  %%               
+                Yhat = zeros(obj.setup.Nfilt+1,obj.setup.dim_out,size(X.y,2));                
+                u_in = [zeros(size(obj.init.input_story(traj).val,1),1), obj.init.input_story(traj).val];
+                Yhat(1,:,:) = obj.setup.measure(X.y,obj.init.params,tspan,u_in(:,(tspan_pos(1):tspan_pos(end))),obj);
+                
+                %%% compute filters %%%
+                if obj.setup.Nfilt > 0                     
+                    
+                    % how long iterate
+                    % odeDD
+%                     tspan_filt = 1+(0:tspan_pos(end)-tspan_pos(1));
+                    % Lsim
+                    tspan_filt = 1+(0:tspan_pos(end)-tspan_pos(1));
+                    
+                    % filter state
+                    x0_filter.val = cell(obj.setup.Nfilt,obj.setup.dim_out);
+                    startpos = 0;
+                    for filt=1:obj.setup.Nfilt 
+                       for dim=1:obj.setup.dim_out
+                           x0_filter.val{filt,dim}(:,max(1,tspan_filt(1)-1)) = x_filters(startpos+1:startpos+obj.setup.filterTF(filt).dim);
+                           startpos = startpos + obj.setup.filterTF(filt).dim;
+                       end
+                    end
+                             
+                    try
+                        [Y, ~] = obj.measure_filter(Yhat(:,:,tspan_filt),tspan_filt,x0_filter);                         
+                    catch                        
+                        J_final = NaN;
+                        break
+                    end                    
+                    
+                    for filt=1:obj.setup.Nfilt
+                        for dim=1:obj.setup.dim_out
+                            % odeDD
+%                             Yhat(filt+1,dim,tspan_filt(2:end)) = Y{filt,dim}.val;
+                            % Lsim                            
+                            Yhat(filt+1,dim,tspan_filt) = Y{filt,dim}.val(tspan_filt);                               
+                        end
+                    end
+                end                                                                              
+                % cost function
+                J = zeros(obj.setup.J_nterm,length(obj.setup.dim_out_compare));
+                target_pos = find(obj.init.Y_space ~= 0);
+
+                for term=1:obj.setup.J_nterm
+                    % get the J
+                    for dim=1:obj.setup.dim_out
+                        tspan_vals = tspan_pos(2:end) - tspan_pos(1) + 1;
+                        target_tmp = reshape((y_target(term,dim,target_pos)),length(tspan_pos)-1,1);
+                        hat_tmp = reshape(Yhat(term,dim,tspan_vals),length(tspan_pos)-1,1);
+                        diff_var = target_tmp-hat_tmp;
+                        J(term,dim) = transpose(diff_var)*diff_var;
+                    end
+
+                end                           
+
+                % scaling
+                J_scaled = zeros(size(J));
+                for dim=1:length(obj.setup.dim_out_compare)
+                    J_scaled(:,obj.setup.dim_out_compare(dim)) = transpose(obj.init.scale_factor_scaled(obj.setup.dim_out_compare(dim),1:obj.setup.J_nterm)).*J(:,obj.setup.dim_out_compare(dim));
+                    Jtot = Jtot + sum(J_scaled(:,obj.setup.dim_out_compare(dim))); 
+                end
+
+                % store terms
+                obj.init.Jterm_store(1:obj.setup.J_nterm) = obj.init.Jterm_store(1:obj.setup.J_nterm) + sum(J_scaled,2);                
+                
+                %%% terminal cost %%%
+                if obj.setup.J_term_terminal
+                    x0 = obj.init.temp_x0(obj.init.traj).val;                    
+                    xterm = x0(obj.setup.terminal_states)-x(obj.setup.terminal_states);
+                    paramsDiff = reshape(xterm,1,length(obj.setup.terminal_states));
+                    J_terminal = paramsDiff*diag(obj.init.scale_factor_scaled_terminal)*transpose(paramsDiff);
+                    
+                    % store terms
+                    obj.init.Jterm_store(end) = J_terminal; 
+                else
+                    J_terminal = 0;
+                end
+                                             
+                % non opt vars barrier term                
+                if obj.setup.bounds                    
+                    for bound=1:length(obj.setup.boundsPos)
+                        % init value
+                        init_value = obj.init.temp_x0(obj.init.traj).val(obj.setup.boundsPos(bound));
+                        % barrier - low
+                        err_low = min(X.y(obj.setup.boundsPos(bound),2:end)-obj.setup.boundsValLow(bound));
+                        % barrier - up
+                        err_up = max(obj.setup.boundsValUp(bound)-X.y(obj.setup.boundsPos(bound),2:end));
+                        % terminal
+                        err_terminal = norm(X.y(obj.setup.boundsPos(bound),:)-init_value);
+                        % sum stuff
+                        if (err_low > 0) && (err_up > 0)
+                            J_barr(bound) = 0*obj.setup.boundsWeight(bound)*err_terminal;
+                        else
+                            J_barr(bound) = 1e5;
+                        end
+                        
+                    end
+                    
+                else
+                    J_barr = 0;
+                end
+                                
+                if any(isinf(J_barr))
+                    J_final = Inf;
+                    break
+                end 
+                
+                %%% Allocation cost fucntion %%%
+                if 0
+                    u_diff = obj.init.input_story(traj).val;
+                    u_diff_norm = obj.init.params.Ru*vecnorm(u_diff).^2;                
+                    J_input = J_input + sum(u_diff_norm);
+                else
+                    J_input = 0;
+                end
+                                
+                J_final = J_final + Jtot + sum(J_barr) + J_terminal + 0*J_input;
+
+                %%% final stuff %%%                
+                obj.init.Yhat_temp = Yhat;
+                
+                currentTime = toc(obj.setup.opt_temp_time);
+                if currentTime > obj.setup.MaxOptTime
+                   J_final = 0;
+                   obj.setup.MaxOptTimeFlag = 1;
+                   return
+                end
+            
+
+            end
+            
+            obj.init.t_J(end+1) = toc(obj.init.t_J_start);  
+        end
+                 
+        %%% LSIM filtering %%%
+        function [Y, X] = measure_filter(varargin)
+                                                  
+            % get varargin
+            obj = varargin{1};
+            U = varargin{2};
+            tspan_pos = varargin{3};
+            X_filter = varargin{4};
+            tspan = obj.setup.time(tspan_pos)-obj.setup.time(tspan_pos(1));
+            
+            obj.init.t_filt_start = tic;
+            
+            % n filters
+            Nfilter = obj.setup.Nfilt;                      
+            
+            if (Nfilter > 0)
+                
+                % iter filters
+                for nfilt=1:Nfilter
+                                        
+                    % iter dim
+                    for dim=1:obj.setup.dim_out 
+                        
+                        % init condition
+                        x0_filter = X_filter.val{nfilt,dim}(:,tspan_pos(1));                        
+
+                        % inputs
+                        u = reshape(U(1,dim,tspan_pos),1,length(tspan_pos));
+                        
+                        if (min(diff(tspan_pos)) ~= 0)                                 
+                            [Y{nfilt,dim}.val, ~, tmp_xtraj] = lsim(obj.setup.filterTF(nfilt).TF,u',tspan,x0_filter);  
+                            X{nfilt,dim}.val = transpose(tmp_xtraj);
+                        else
+                            Y{nfilt,dim}.val = 0;
+                            X{nfilt,dim}.val = x0_filter;
+                        end
+                    end
+                end
+            else
+                Y = 0;
+                X = 0;
+            end
+            
+            obj.init.t_filt(end+1) = toc(obj.init.t_filt_start);
+            
+        end
+        
+        % dJ_cond: function for the adaptive sampling
+        % test: try to work with wavelets
+        % it will simply return the main frequencies of the current signal
+        function obj = dJ_cond_v5_function(obj)
+
+            % Wavelet adaptive
+            buffer_ready = (~obj.init.PE_flag)*(obj.init.ActualTimeIndex > obj.init.FNts*obj.init.Fbuflen);
+            buf_data = squeeze(obj.init.Y(obj.init.traj).val(1,:,:));
+            y = obj.init.Y_full_story(obj.init.traj).val(1,:,obj.init.ActualTimeIndex);
+            DiffTerm = diff(buf_data);
+            VecTerm = vecnorm(DiffTerm,2,2);
+            obj.init.PE_story(:,obj.init.ActualTimeIndex) = sum(VecTerm) + norm(y'-buf_data(end,:));            
+
+            if buffer_ready && obj.setup.AdaptiveSampling                
+
+                % get current buffer - new
+                pos_hat = obj.init.ActualTimeIndex:-obj.init.FNts:(obj.init.ActualTimeIndex-obj.init.FNts*obj.init.Fbuflen);                
+                short_win_data = squeeze(obj.init.Y_full_story(obj.init.traj).val(1,obj.init.wavelet_output_dim,pos_hat));                
+
+                % set data                
+                short_win_data = reshape(short_win_data,numel(obj.init.wavelet_output_dim),numel(pos_hat));
+                buffer = vecnorm(short_win_data,2,1);                 
+                                                                                
+                % frequency constraint                                
+                %[WT, F] = cwt(buffer,obj.init.wvname,1/obj.setup.Ts,'VoicesPerOctave',obj.init.Nv,'FrequencyLimits',obj.init.FLIMITS);                
+                [WT, ~] = cwt(buffer,obj.init.wvname,1/obj.setup.Ts,'VoicesPerOctave',obj.init.Nv);                
+
+
+                % real values
+                WT_real = real(WT);
+                WT_norm = vecnorm(WT_real,2,1);   
+                WT_norm_filt = movmean(WT_norm,5);                
+                %F = scal2frq(WT_norm_filt,'morl',obj.setup.Ts);
+                % find derivative
+                [WT_norm_peaks,pos_peaks] = findpeaks(WT_norm_filt);                
+                % set freqs
+                if ~isempty(pos_peaks)
+                    % pos max and min on the WT
+                    [pos_max] = find(WT_norm_filt == max(WT_norm_peaks),1,'first');
+                    [pos_min] = find(WT_norm_filt == min(WT_norm_peaks),1,'first');
+
+                    % store max min F
+                    %obj.init.Fcwt_story(:,obj.init.ActualTimeIndex) = [F(pos_max) F(pos_min)];
+
+                    % pos max and min on the F
+                    % new                    
+                    F_def(1,1) = 2*pi*WT_norm_filt(pos_max);  %1 max
+                    F_def(2,1) = 2*pi*WT_norm_filt(pos_min);  %2 min
+                       
+                    if min(F_def) < obj.init.Fmin
+                        F_def = zeros(obj.init.nfreqs,1);
+                    end
+
+                    % select freqs
+                    obj.init.freqs = [obj.init.freqs F_def];
+                else
+                    obj.init.freqs = [obj.init.freqs zeros(obj.init.nfreqs,1)];
+                end                                                          
+            else
+                obj.init.freqs = [obj.init.freqs zeros(obj.init.nfreqs,1)];
+            end  
+
+            % PE adaptive
+            if obj.init.PE_flag
+                buf_data = squeeze(obj.init.Y(obj.init.traj).val(1,:,:));
+                y = reshape(obj.init.Y_full_story(obj.init.traj).val(1,:,obj.init.ActualTimeIndex),obj.init.params.OutDim,1);
+                DiffTerm = diff(buf_data);
+                VecTerm = vecnorm(DiffTerm,2,2);
+                obj.init.PE_story(:,obj.init.ActualTimeIndex) = sum(VecTerm) + norm(y-buf_data(end,:));
+                obj.init.freqs = [obj.init.freqs ones(obj.init.nfreqs,1)];
+            end
+
+        end                
+        
+        % target function (observer or control design)
+        function obj = target(obj)          
+            for i=1:obj.setup.Ntraj
+                obj.init.target(i).val = obj.init.Y(i).val;
+            end
+        end               
+        
+        % observer function: this method wraps up all the afromentioned
+        % methods and actually implements the observer. Check the reference
+        % for more information. 
+        function obj = observer(obj,xhat,y)
+            
+            % get estimations
+            for traj=1:obj.setup.Ntraj
+                obj.init.traj = traj;
+                xhat_tmp(traj).val = xhat(traj).val(:,obj.init.ActualTimeIndex);
+            end
+            xhat = xhat_tmp;
+            
+            obj.init.just_optimised = 0;
+            
+            for traj=1:obj.setup.Ntraj
+                obj.init.traj = traj;
+                % save runtime state
+                obj.init.X_est_runtime(traj).val(:,obj.init.ActualTimeIndex) = obj.init.X_est(traj).val(:,obj.init.ActualTimeIndex);
+                % get ESTIMATED measure from ESTIMATED state (xhat)
+                yhat(traj).val = obj.setup.measure(xhat(traj).val,obj.init.params,obj.setup.time(obj.init.ActualTimeIndex),obj.init.input_story(traj).val(:,max(1,obj.init.ActualTimeIndex-1)),obj);
+            end
+            
+            for traj=1:obj.setup.Ntraj
+                obj.init.traj = traj;
+                % get filters - y
+                obj.init.Y_full_story(traj).val(1,:,obj.init.ActualTimeIndex) = y(traj).val;                 
+                tspan_pos = [max(1,obj.init.ActualTimeIndex-1), obj.init.ActualTimeIndex];
+                [dy, x_filter] = obj.measure_filter(obj.init.Y_full_story(traj).val,tspan_pos,obj.init.X_filter(obj.init.traj));                
+                for filt=1:obj.setup.Nfilt
+                    for dim=1:obj.setup.dim_out
+                        y_tmp(dim,1) = dy{filt,dim}.val(end);
+                        obj.init.X_filter(traj).val{filt,dim}(:,unique(tspan_pos)) = x_filter{filt,dim}.val;
+                    end   
+                    y(traj).val = [y(traj).val, y_tmp];
+                end                
+                % get filters - yhat
+                obj.init.Yhat_full_story(traj).val(1,:,obj.init.ActualTimeIndex) = yhat(traj).val;
+                tspan_pos = [max(1,obj.init.ActualTimeIndex-1), obj.init.ActualTimeIndex];
+                [dyhat, x_filter] = obj.measure_filter(obj.init.Yhat_full_story(traj).val,tspan_pos,obj.init.X_filter_est(obj.init.traj));
+                for filt=1:obj.setup.J_nterm-1
+                    for dim=1:obj.setup.dim_out
+                        y_tmp(dim,1) = dyhat{filt,dim}.val(end);                        
+                        obj.init.X_filter_est(traj).val{filt,dim}(:,unique(tspan_pos)) = x_filter{filt,dim}.val;
+                    end
+                    yhat(traj).val = [yhat(traj).val, y_tmp];                    
+                end
+            end            
+            
+            %%%%%%%%%%%%%%%%%%%%% INSERT OPT %%%%%%%%%%%%%%%%%%%%%
+            for term=1:obj.setup.J_nterm
+                for traj=1:obj.setup.Ntraj
+                    obj.init.traj = traj;
+                    obj.init.Y_full_story(traj).val(term,:,obj.init.ActualTimeIndex) = y(traj).val(:,term);                    
+                    obj.init.Yhat_full_story(traj).val(term,:,obj.init.ActualTimeIndex) = yhat(traj).val(:,term);
+                    obj.init.Yhat_full_story_runtime(traj).val(term,:,obj.init.ActualTimeIndex) = yhat(traj).val(:,term);
+                end
+            end                        
+
+            % fisrt bunch of data - read Y every Nts and check if the signal is
+            distance = obj.init.ActualTimeIndex-obj.init.Y_space(end);   
+            NtsPos = mod(obj.init.Nsaved,obj.setup.w)+1;
+            obj.init.NtsPos = NtsPos;
+
+            % if the current Nts is different from the standard one, than
+            % it measn that a complete cycle has been done, and you should
+            % replace it again with the original one
+            if obj.setup.NtsVal(NtsPos) ~= obj.setup.Nts
+                obj.setup.NtsVal(NtsPos) = obj.setup.Nts;
+            end
+            
+            % set optimization time and iterations depending on the current Nts
+            if obj.setup.NtsVal(NtsPos) == min(obj.setup.NtsVal)
+                obj.setup.max_iter = min(obj.setup.max_iterVal);
+                if ~isinf(obj.setup.MaxOptTime)
+                    obj.setup.MaxOptTime = 0.2*obj.setup.NtsVal(NtsPos)*obj.setup.Ts;
+                end
+            else
+                obj.setup.max_iter = max(obj.setup.max_iterVal);
+                if ~isinf(obj.setup.MaxOptTime)
+                    obj.setup.MaxOptTime = 0.2*obj.setup.NtsVal(NtsPos)*obj.setup.Ts;
+                end
+            end            
+            
+            obj = obj.dJ_cond_v5_function();             
+            % define selcted freq: freq_sel=1 MAX freq_sel=2 MIN
+            freq_sel = obj.init.Fselect;
+            % define bound on freq (at least 2 due to Nyquist)
+            freq_bound = obj.init.Fnyq;
+            % set NtsVal depending on freqs
+            if obj.setup.AdaptiveSampling
+                % define freq on which calibrate the sampling time
+
+                lasterr = nonzeros(squeeze(obj.init.Y(obj.init.traj).val(1,:,:))) - nonzeros(squeeze(obj.init.Yhat_full_story(obj.init.traj).val(1,:,nonzeros(obj.init.Y_space))));
+                if ~isempty(lasterr)
+                    lasterr = vecnorm(lasterr);
+                else
+                    lasterr = Inf;
+                end
+                obj.init.lasterror_story(obj.init.traj).val(obj.init.ActualTimeIndex) = lasterr;
+
+                % here with wavelets
+                if (~obj.init.PE_flag)
+                    if (lasterr > obj.init.epsD)                        
+                        freq = freq_bound*obj.init.freqs(freq_sel,end); % Hz
+                        Ts_wv = 1/(freq); % s
+                        distance_min = max(1,ceil(Ts_wv/obj.setup.Ts));
+                    else
+                        distance_min = Inf;
+                    end
+                end
+
+                % here with PE
+                if (obj.init.PE_flag)
+                    if (obj.init.PE_story(obj.init.ActualTimeIndex) >= freq_bound) && (lasterr > obj.init.epsD)
+                        distance_min = distance;
+                    else
+                        distance_min = Inf;
+                    end
+                end
+
+                if ~any(obj.init.freqs(:,end)) 
+                    distance_min = Inf;%obj.setup.NtsVal(NtsPos); 
+                else
+                    a = 1;
+                end
+                obj.init.dmin_story(obj.init.ActualTimeIndex) = distance_min;
+
+            else
+                distance_min = obj.setup.NtsVal(NtsPos);
+            end
+
+            % update the minimum distance                        
+            obj.setup.NtsVal(NtsPos) = distance_min;            
+            
+            % safety flag
+            obj.init.distance_safe_flag = (distance < obj.init.safety_interval);   
+            obj.init.distance_safe_flag_story(obj.init.traj).val(obj.init.ActualTimeIndex) = obj.init.distance_safe_flag;
+            
+            %%%% observer %%%%
+            if  ( ~( (distance < obj.setup.NtsVal(NtsPos)) && (obj.init.distance_safe_flag) ) )
+
+                if obj.setup.print
+                    % Display iteration slengthtep
+                    disp(['n window: ', num2str(obj.setup.w),'  n samples: ', num2str(obj.setup.Nts)])                    
+                    disp(['N. optimisations RUN: ',num2str(obj.init.opt_counter)]);
+                    disp(['N. optimisations SELECTED: ',num2str(obj.init.select_counter)]);                    
+                end                                
+               
+                %%%% OUTPUT measurements - buffer of w elements
+                % measures
+                for term=1:obj.setup.J_nterm
+                    for traj=1:obj.setup.Ntraj
+                        obj.init.traj = traj;
+                        obj.init.Y(traj).val(term,:,1:end-1) = obj.init.Y(traj).val(term,:,2:end);
+                        obj.init.Y(traj).val(term,:,end) = y(traj).val(:,term);
+                    end
+                end
+
+                % adaptive sampling                
+                obj.init.Y_space(1:end-1) = obj.init.Y_space(2:end);
+                obj.init.Y_space(end) = obj.init.ActualTimeIndex;
+                obj.init.Y_space_full_story(end+1) = obj.init.ActualTimeIndex;
+                
+                obj.init.Nsaved = obj.init.Nsaved + 1;
+                
+                % store measure times
+                obj.init.temp_time = [obj.init.temp_time obj.init.ActualTimeIndex];
+
+                % check only on the first traj as the sampling is coherent
+                % on the 2.
+                % WaitAlBuffer == 0 --> start asap
+                if obj.setup.WaitAllBuffer == 0
+                    cols_nonzeros = length(find(obj.init.Y_space ~= 0))*obj.setup.dim_out*nnz(obj.setup.J_temp_scale);                
+                % WaitAlBuffer == 1 --> start when all the buffer is full
+                elseif obj.setup.WaitAllBuffer == 1
+                    cols_nonzeros = length(find(obj.init.Y_space ~= 0));  
+                % WaitAlBuffer == 2 --> start at the last step of the trajectory
+                elseif obj.setup.WaitAllBuffer == 2
+                    cols_nonzeros = obj.setup.Nts;
+                else
+                    error('Wrong Wait Buffer Option')
+                end
+
+                % flag
+                % WaitAlBuffer == 0 --> start asap
+                if obj.setup.WaitAllBuffer == 0
+                    flag = 2*length(obj.setup.opt_vars)+1; % Aeyels condition (see https://doi.org/10.48550/arXiv.2204.09359)
+                % WaitAlBuffer == 1 --> start when all the buffer is full
+                elseif obj.setup.WaitAllBuffer == 1
+                    flag = obj.setup.w;
+                % WaitAlBuffer == 2 --> start at the last step of the trajectory
+                elseif obj.setup.WaitAllBuffer == 2
+                    flag = obj.setup.Niter-obj.init.ActualTimeIndex;
+                else
+                    error('Wrong Wait Buffer Option')
+                end
+                % real
+                if cols_nonzeros >= flag                                        
+
+                    if obj.setup.WaitAllBuffer == 2
+                        obj.setup.Niter = obj.init.ActualTimeIndex;
+                        obj.setup.time = obj.setup.time(1:obj.init.ActualTimeIndex);
+                        obj.init.break = 1;
+                        obj.setup.w = numel(obj.init.Y_space_full_story)-1;
+                        obj.init.Y_space = nonzeros(obj.init.Y_space_full_story)';
+                        for traj=1:obj.init.params.Ntraj
+                            obj.init.Y(traj).val = obj.init.Y_full_story(traj).val(:,:,obj.init.Y_space);
+                        end                        
+                    end
+
+                    if obj.setup.forward
+
+                        % get the maximum distance
+                        first_nonzero = find(obj.init.Y_space,1,'first');
+                        Y_space_nonzero = obj.init.Y_space(first_nonzero:end);
+                        max_dist = max(diff(Y_space_nonzero));
+                        if isempty(max_dist)
+                            max_dist = 1;
+                        end
+                        
+                        % setup buffers
+                        n_samples = min(length(obj.init.Y_space_full_story)-1,obj.setup.w);
+                        buf_Y_space_full_story = obj.init.Y_space_full_story(end-n_samples:end);
+                        
+                        %%%% FLUSH THE BUFFER IF SAFETY FLAG %%%%
+                        if (obj.setup.FlushBuffer) && (max_dist >= obj.init.safety_interval)
+                            
+                            % buffer adaptive sampling: these buffers keep track of the
+                            % time instants in which the measured data have been stored. 
+                            step = 1*obj.setup.Nts;
+                            start = obj.init.Y_space(end)-step*(obj.setup.w-1); 
+                            stop = obj.init.Y_space(end);
+                            obj.init.Y_space = start:step:stop;
+                            
+                            % down samplingbuffer
+                            for traj=1:obj.setup.Ntraj
+                                obj.init.traj = traj;
+                                obj.init.Y(traj).val =  obj.init.Y_full_story(traj).val(:,:,obj.init.Y_space);
+                            end
+                            
+                            % update full story
+                            obj.init.Y_space_full_story(end) = [];
+                            obj.init.Y_space_full_story = [obj.init.Y_space_full_story, obj.init.Y_space];
+                            
+                            % set again (used for back_time)
+                            buf_Y_space_full_story = obj.init.Y_space_full_story(end-1*obj.setup.w+1:end);
+                            
+                            % back time index
+                            buf_dist = diff(buf_Y_space_full_story);                        
+                            obj.init.BackTimeIndex = obj.setup.time(max(obj.init.ActualTimeIndex-sum(buf_dist(1:end)),1));
+                        else                            
+                            % back time index
+                            buf_dist = diff(buf_Y_space_full_story);
+                            obj.init.BackTimeIndex = obj.setup.time(max(obj.init.ActualTimeIndex-sum(buf_dist(1:end)),1)); 
+                        end
+                        
+                        
+                        obj.init.BackIterIndex = find(obj.setup.time==obj.init.BackTimeIndex);                        
+
+                        % set of initial conditions
+                        for traj=1:obj.setup.Ntraj
+                            obj.init.traj = traj;
+                            % start from the correct one                             
+                            obj.init.temp_x0_nonopt(traj).val = obj.init.X_est(traj).val(obj.setup.nonopt_vars,obj.init.BackIterIndex);
+                            % change only the values which are to be optimised
+                            % only 1 set of vars regardless to the number
+                            % of trajectories used as we're not estimating
+                            % the state or the model parameters
+                            obj.init.temp_x0_opt = obj.init.X_est(1).val(obj.setup.opt_vars,obj.init.BackIterIndex);
+                            
+                            % if filters are used, their state shall be
+                            % treated as non optimised vars as well
+                            x0_filters = [];
+                            filterstartpos = max(1,obj.init.BackIterIndex);
+                            for nfilt=1:obj.setup.Nfilt
+                                for dim=1:obj.setup.dim_out
+                                    tmp = reshape(obj.init.X_filter(traj).val{nfilt,dim}(:,filterstartpos),1,obj.setup.filterTF(nfilt).dim);                                    
+                                    x0_filters = [x0_filters, tmp];
+                                end
+                            end
+                            obj.init.temp_x0_filters(traj).val = x0_filters;
+                            Lfilt = length(x0_filters);
+                             
+                            % reconstruct temp_x0 from opt/nonopt vars
+                            obj.init.temp_x0(traj).val = zeros(obj.setup.dim_state,1);
+                            obj.init.temp_x0(traj).val(obj.setup.opt_vars) = obj.init.temp_x0_opt;
+                            obj.init.temp_x0(traj).val(obj.setup.nonopt_vars) = obj.init.temp_x0_nonopt(traj).val;
+                            obj.init.temp_x0(traj).val(end+1:end+Lfilt) = obj.init.temp_x0_filters(traj).val;
+                        end
+                        
+                        % set target
+                        obj = obj.target();
+                        for traj = 1:obj.setup.Ntraj
+                            obj.init.traj = traj;
+                            nonzero_space = find(obj.init.Y_space ~= 0);
+                            nonzero_pos = obj.init.Y_space(nonzero_space);
+                            obj.init.target_story(traj).val(:,:,nonzero_pos) = obj.init.target(traj).val(:,:,nonzero_space);
+                        end
+                        
+                        %%% normalisation %%%
+                        if (obj.setup.J_normalise) && (~obj.init.normalised) 
+                            range = 1:obj.init.ActualTimeIndex;
+                            for filt=1:obj.setup.J_nterm
+                                for dim=1:length(obj.setup.dim_out_compare)
+                                    for traj=1:obj.setup.Ntraj
+                                        E = (obj.init.Yhat_full_story(traj).val(filt,obj.setup.dim_out_compare(dim),range) - ...
+                                            obj.init.Y_full_story(traj).val(filt,obj.setup.dim_out_compare(dim),range)).^2;
+                                        E = reshape(E,1,size(E,3));
+                                        Emax(traj) = max(E);
+                                        if Emax(traj) == 0 || (abs(Emax(traj)) < 1e-15)
+                                            Emax(traj) = 1;
+                                        end                                        
+                                    end
+                                    Emax = max(Emax);
+                                    obj.init.scale_factor_scaled(obj.setup.dim_out_compare(dim),filt) = obj.init.scale_factor(obj.setup.dim_out_compare(dim),filt)/Emax;
+                                end
+                            end        
+                            if (obj.setup.J_term_terminal) && (obj.setup.terminal_normalise)
+                                E = vecnorm(obj.init.X_est(traj).val(obj.setup.terminal_states,range)',2,1);
+                                E_scale = E/sum(E);
+                                for dim=1:length(obj.setup.terminal_states)                                                                        
+%                                     obj.init.scale_factor_scaled_terminal(dim) = obj.init.scale_factor(1,obj.setup.J_term_terminal_position)/(E_scale(dim)); 
+                                    if E(dim) ~= 0
+                                        obj.init.scale_factor_scaled_terminal(dim) = obj.init.scale_factor(1,obj.setup.J_term_terminal_position)/E(dim);
+                                    else
+                                        obj.init.scale_factor_scaled_terminal(dim) = 1;
+                                    end
+                                end    
+                            elseif (obj.setup.J_term_terminal)
+                                for dim=1:length(obj.setup.terminal_states)
+                                    obj.init.scale_factor_scaled_terminal(dim) = obj.init.scale_factor(1,obj.setup.J_term_terminal_position);
+                                end
+                            end   
+                            if (~isempty(obj.setup.terminal_weights)) && (obj.setup.J_term_terminal)
+                                for dim=1:length(obj.setup.terminal_states)
+                                    obj.init.scale_factor_scaled_terminal(dim) = obj.init.scale_factor_scaled_terminal(dim)*obj.setup.terminal_weights(dim);
+                                end
+                            end
+                            obj.init.normalised = 1;
+                        elseif (~obj.setup.J_normalise)
+                            obj.init.scale_factor_scaled = obj.init.scale_factor;
+                            if obj.setup.J_term_terminal
+                                for dim=1:length(obj.setup.terminal_states)
+                                    obj.init.scale_factor_scaled_terminal(dim) = obj.init.scale_factor(1,obj.setup.J_term_terminal_position)*obj.setup.terminal_weights(dim);
+                                end
+                            end
+                        end
+                        
+                        % check fmin time (boundaries)
+                        obj.setup.opt_temp_time = tic;
+                        obj.setup.MaxOptTimeFlag = 0;
+                        
+                        % save max times
+                        obj.init.MaxOptTime_story = [obj.init.MaxOptTime_story obj.setup.MaxOptTime];
+                        obj.init.MaxIter_story = [obj.init.MaxIter_story obj.setup.max_iter];
+                        
+                        if (obj.setup.optimise)
+                            
+                            % flag for first opt
+                            obj.init.FirstOpt = 1;
+                            
+                            % save J before the optimisation to confront it later 
+                            [J_before, obj_tmp] = obj.setup.cost_run(obj.init.temp_x0_opt,obj.init.temp_x0_nonopt,obj.init.temp_x0_filters,obj.init.target);
+
+
+                            % Optimisation (only if distance_safe_flag == 1)
+                            opt_time = tic;                        
+
+                            %%%%% OPTIMISATION - NORMAL MODE %%%%%%                            
+                            % run optimization
+                            if obj.setup.MultiStart                                                                 
+                                % init multistart
+                                ms = MultiStart('FunctionTolerance',obj.init.TolFun, 'XTolerance', obj.init.TolX, 'UseParallel',false);
+                            end                                                                   
+                            % create problem
+                            try
+                                problem = createOptimProblem(func2str(obj.setup.fmin),'objective',@(x)obj.setup.cost_run(x,obj.init.temp_x0_nonopt,obj.init.temp_x0_filters,obj.init.target,1),...
+                                                                            'x0', obj.init.temp_x0_opt, 'Aineq', obj.setup.Acon, 'bineq', obj.setup.Bcon, 'Aeq', obj.setup.Acon_eq, 'beq', obj.setup.Bcon_eq, ...
+                                                                            'lb', obj.setup.LBcon, 'ub', obj.setup.UBcon, 'nonlcon', @(x)obj.setup.NONCOLcon(x,obj.init.temp_x0_nonopt,obj), 'options', obj.init.myoptioptions);
+                                if ~obj.setup.MultiStart
+                                    [NewXopt, J] = obj.setup.fmin(problem);
+                                else
+                                    [NewXopt, J] = run(ms,problem,obj.init.params.tpoints);                                                                
+                                end
+                                
+                            catch
+                                problem = createOptimProblem('fmincon','objective',@(x)obj.setup.cost_run(x,obj.init.temp_x0_nonopt,obj.init.temp_x0_filters,obj.init.target,1),...
+                                                                            'x0', obj.init.temp_x0_opt, 'Aineq', obj.setup.Acon, 'bineq', obj.setup.Bcon, 'Aeq', obj.setup.Acon_eq, 'beq', obj.setup.Bcon_eq, ...
+                                                                            'lb', obj.setup.LBcon, 'ub', obj.setup.UBcon, 'nonlcon', @(x)obj.setup.NONCOLcon(x,obj.init.temp_x0_nonopt,obj), 'options', obj.init.myoptioptions);
+                                                                        
+                                if strcmp(func2str(obj.setup.fmin),'patternsearch')                                                             
+                                    problem.solver = 'patternsearch';   
+                                    obj.init.myoptioptions.ConstraintTolerance = 1e-10;
+                                    obj.init.myoptioptions.ScaleMesh = 'off';
+                                    obj.init.myoptioptions.MaxMeshSize = 100;
+                                    obj.init.myoptioptions.InitialMeshSize = 100;
+                                    obj.init.myoptioptions.Display = 'iter';
+                                    obj.init.myoptioptions.Algorithm = 'nups-gps';
+                                    obj.init.myoptioptions.UseParallel = true;                                    
+                                    obj.init.myoptimoptions.UseCompletePoll = true;
+                                elseif strcmp(func2str(obj.setup.fmin),'fminsearchcon')   
+                                    problem = createOptimProblem('fmincon','objective',@(x)obj.setup.cost_run(x,obj.init.temp_x0_nonopt,obj.init.temp_x0_filters,obj.init.target,1),...
+                                                                            'x0',  obj.init.temp_x0_opt, 'lb', obj.setup.LBcon, 'ub', obj.setup.UBcon, 'Aeq', obj.setup.Acon_eq, 'beq', obj.setup.Bcon_eq, ...
+                                                                            'nonlcon', @(x)obj.setup.NONCOLcon(x,obj.init.temp_x0_nonopt,obj), 'options', obj.init.myoptioptions);
+                                    problem = rmfield(problem,'bineq');
+                                    problem = rmfield(problem,'Aineq');   
+                                    obj.init.myoptioptions.ConstraintTolerance = 1e-10;
+                                    problem.options = obj.init.myoptioptions;
+                                    problem.solver = 'fminsearchcon';                                    
+                                else    
+                                    error('WRONG OPTIMISATION SETUP');
+                                end
+                                
+                                problem.options = obj.init.myoptioptions;  
+                                
+                                if ~obj.setup.MultiStart
+                                    try
+                                        [NewXopt, J] = obj.setup.fmin(problem);
+                                    catch ME
+                                        [NewXopt, J] = obj.setup.fmin(problem.objective,problem.x0,problem.lb,problem.ub,problem.Aeq,problem.beq,problem.nonlcon,problem.options);
+                                    end
+                                else
+                                    list = obj.init.params.tpoints.list;
+                                    for pp = 1:obj.init.params.tpoints.NumStartPoints
+                                        problem.x0 = list(pp,:);
+                                        [J_before(pp), obj_tmp] = obj.setup.cost_run(problem.x0,obj.init.temp_x0_nonopt,obj.init.temp_x0_filters,obj.init.target);   
+                                        try
+                                            [NewXopt(pp,:), J(pp,:)] = obj.setup.fmin(problem);
+                                        catch ME
+                                            [NewXopt(pp,:), J(pp,:)] = obj.setup.fmin(problem.objective,problem.x0,problem.lb,problem.ub,problem.Aeq,problem.beq,problem.nonlcon,problem.options);
+                                        end
+                                    end
+                                    %J_improve = J./J_before';
+                                    [J,pos] = min(J);
+                                    J_before = J_before(pos);
+                                    NewXopt = NewXopt(pos,:);
+                                end
+                            end
+                            
+                            % reconstruct NewXopt from opt/nonopt vars
+                            NewXopt_tmp = [];
+                            for traj = 1:obj.setup.Ntraj
+                                obj.init.traj = traj;
+                                NewXopt_end = zeros(obj.setup.dim_state,1);
+                                NewXopt_end(obj.setup.opt_vars) = NewXopt;
+                                NewXopt_end(obj.setup.nonopt_vars) = obj.init.temp_x0_nonopt(traj).val;                          
+                                NewXopt_tmp(traj).val = NewXopt_end;                           
+                            end
+                            % set new state
+                            NewXopt = NewXopt_tmp;                                                
+
+                            % opt counter
+                            if traj == 1
+                                obj.init.opt_counter = obj.init.opt_counter + 1;
+                            end
+
+                            % check J_dot condition
+                            J_diff = (J/J_before);
+
+                            if (obj.setup.AlwaysOpt) || ( (J_diff <= obj.setup.Jdot_thresh) || (distance > obj.init.safety_interval) )                                                        
+
+                                % on each trajectory
+                                for traj=1:obj.setup.Ntraj
+
+                                    % set traj
+                                    obj.init.traj = traj;
+
+                                    % update state
+                                    obj.init.X_est(traj).val(:,obj.init.BackIterIndex) = NewXopt(traj).val;
+
+                                    % store measure times
+                                    obj.init.opt_chosen_time = [obj.init.opt_chosen_time obj.init.ActualTimeIndex];
+                                    obj.init.just_optimised = 1;
+
+                                    % counters
+                                    obj.init.jump_flag = 0;
+                                    obj.init.select_counter = obj.init.select_counter + 1;
+
+                                    x_propagate = NewXopt(traj).val;
+
+                                    % update params
+                                    obj.init.params = obj.setup.params.params_update(obj.init.params,x_propagate);
+
+                                    %%%%%%%%%%%%%%%%% FIRST MEASURE UPDATE %%%%%%%%
+                                    % manage measurements
+                                    back_time = obj.init.BackIterIndex;
+
+                                    %%%% ESTIMATED measurements
+                                    % measures       
+                                    % NB: the output storage has to be done in
+                                    % back_time+1 as the propagation has been
+                                    % performed 
+                                    Yhat = obj.setup.measure(x_propagate,obj.init.params,obj.setup.time(back_time),obj.init.input_story(traj).val(:,back_time),obj);
+                                    % get filters - yhat
+                                    obj.init.Yhat_full_story(traj).val(1,:,back_time) = Yhat;  
+                                    tspan_pos = [max(1,back_time-1), back_time];
+                                    [dyhat, x_filter] = obj.measure_filter(obj.init.Yhat_full_story(traj).val,tspan_pos,obj.init.X_filter_est(obj.init.traj));                                
+                                    yhat(traj).val = Yhat;
+                                    for filt=1:obj.setup.Nfilt
+                                        for dim=1:obj.setup.dim_out
+                                            y_tmp(dim,1) = dyhat{filt,dim}.val(end);
+                                            obj.init.X_filter_est(traj).val{filt,dim}(:,unique(tspan_pos)) = x_filter{filt,dim}.val;
+                                        end
+                                        yhat(traj).val = [yhat(traj).val, y_tmp];
+                                    end
+
+
+                                    for term=1:obj.setup.J_nterm
+                                        obj.init.Yhat_full_story(traj).val(term,:,back_time) = yhat(traj).val(:,term);
+                                    end
+                                    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+                                    %%%%%%%%%%% PROPAGATION %%%%%%%%%%%%%%%%%%%%%%%
+                                    n_iter_propagate = obj.init.ActualTimeIndex-back_time;
+                                    if ~strcmp(func2str(obj.setup.ode),'odeLsim')
+                                        
+                                        for j=1:n_iter_propagate
+                                            
+                                            % back time
+                                            back_time = obj.init.BackIterIndex+j;
+                                            tspan = obj.setup.time(back_time-1:back_time);                                            
+
+                                            % how do you handle the input?
+                                            obj.init.params.ActualTimeIndex = back_time; % here you have the -1 because BackIterIndex is differently set up than in measure_function                                                                         
+
+                                            % integrate
+                                            obj.init.t_ode_start = tic;                     
+                                            X = obj.setup.ode(@(t,x)obj.setup.model(t, x, obj.init.params, obj), tspan, x_propagate,obj.setup.odeset);                                    
+                                            x_propagate = X.y(:,end);                      
+                                            obj.init.X_est(traj).val(:,back_time) = x_propagate;
+                                            obj.init.t_ode(end+1) = toc(obj.init.t_ode_start);
+                                            
+                                        end
+                                        
+                                    else
+                                        
+                                        tspan = obj.setup.time(back_time:obj.init.ActualTimeIndex);
+                                        % integrate
+                                        obj.init.t_ode_start = tic;                     
+                                        X = obj.setup.ode(@(t,x)obj.setup.model(t, x, obj.init.params, obj), tspan, x_propagate,obj.setup.odeset);                                    
+                                        x_propagate = X.y(:,end);                      
+                                        obj.init.X_est(traj).val(:,back_time:obj.init.ActualTimeIndex) = X.y;
+                                        obj.init.t_ode(end+1) = toc(obj.init.t_ode_start);
+                                        
+                                    end
+
+                                    for j =1:n_iter_propagate                                                                                
+
+                                        % back time
+                                        back_time = obj.init.BackIterIndex+j;                                        
+
+                                        % how do you handle the input?
+                                        obj.init.params.ActualTimeIndex = back_time; % here you have the -1 because BackIterIndex is differently set up than in measure_function                                          
+                                        
+                                        if strcmp(func2str(obj.setup.ode),'odeLsim')                                        
+                                            x_propagate = X.y(:,back_time);
+                                        else
+                                            x_propagate = obj.init.X_est(traj).val(:,back_time);
+                                        end
+
+                                        %%%% ESTIMATED measurements
+                                        % measures       
+                                        % NB: the output storage has to be done in
+                                        % back_time+1 as the propagation has been
+                                        % performed 
+                                        Yhat = obj.setup.measure(x_propagate,obj.init.params,obj.setup.time(back_time),obj.init.input_story(traj).val(:,back_time-1),obj);
+                                        % get filters - yhat
+                                        obj.init.Yhat_full_story(traj).val(1,:,back_time) = Yhat;            
+                                        tspan_pos = [max(1,back_time-1), back_time];
+                                        [dyhat, x_filter] = obj.measure_filter(obj.init.Yhat_full_story(traj).val,tspan_pos,obj.init.X_filter_est(obj.init.traj));                                
+                                        yhat(traj).val = Yhat;
+                                        for filt=1:obj.setup.Nfilt
+                                            for dim=1:obj.setup.dim_out
+                                                y_tmp(dim,1) = dyhat{filt,dim}.val(end);
+                                                obj.init.X_filter_est(traj).val{filt,dim}(:,unique(tspan_pos)) = x_filter{filt,dim}.val;
+                                            end
+                                            yhat(traj).val = [yhat(traj).val, y_tmp]; 
+                                        end                                    
+                                        for term=1:obj.setup.J_nterm
+                                            obj.init.Yhat_full_story(traj).val(term,:,back_time) = yhat(traj).val(:,term);                                        
+                                        end
+                                    end
+                                    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
+                                    obj.init.Jstory(1,end+1) = J;
+                                    if obj.setup.Jterm_store
+                                        obj.init.Jterm_story(:,end+1) = obj_tmp.init.Jterm_store;
+                                    end                              
+                                end
+                            else
+                                % on each trajectory
+                                for traj=1:obj.setup.Ntraj
+                                    obj.init.traj = traj;
+                                    % keep the initial guess
+                                    obj.init.X_est(traj).val(obj.setup.opt_vars,obj.init.BackIterIndex) = obj.init.temp_x0_opt;
+                                end
+
+                                % restore params
+                                obj.init.params = obj.setup.params.params_update(obj.init.params,obj.init.temp_x0(traj).val);                                                        
+                            end
+
+                            % stop time counter
+                            obj.init.opt_time(end+1) = toc(opt_time);
+
+                        end
+                    end
+
+                else                    
+
+                end
+
+                if obj.setup.print
+                    clc;
+                end                
+            else
+            end            
+        end
+    end
+end
diff --git a/Lib/obs/wrapper_observer.m b/Lib/obs/wrapper_observer.m
new file mode 100644
index 0000000..8ffaf40
--- /dev/null
+++ b/Lib/obs/wrapper_observer.m
@@ -0,0 +1,19 @@
+function xhat = wrapper_observer(t,x,y,Time,obs) 
+%#codegen
+
+    % get time index
+    tdiff = Time-t;   
+    pos = find(abs(tdiff) == min(abs(tdiff)),1,'first'); 
+    pos = min(pos,length(Time));
+
+    % set time index
+    obs.init.ActualTimeIndex = pos;
+    
+    % set output
+    y_meas.val = y;
+    
+    % call the observer
+    obs = obs.observer(x,y_meas);
+    
+    xhat = obs.init.X_est.val(:,pos);
+end
\ No newline at end of file
diff --git a/Lib/simulation_scripts/TCV/simulation_TCV.m b/Lib/simulation_scripts/TCV/simulation_TCV.m
new file mode 100644
index 0000000..36f56bf
--- /dev/null
+++ b/Lib/simulation_scripts/TCV/simulation_TCV.m
@@ -0,0 +1,261 @@
+%% SIMULATION_GENERAL_V3
+% file: simulation_general_v3.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: function to setup and use the MHE observer on general model
+% INPUT: none
+% OUTPUT: params,obs
+function [obs, params] = simulation_TCV
+
+%%%% Init Section %%%%
+% uncomment to close previously opened figures
+% close all
+
+% rng('default');
+rng(1);
+    
+% init observer buffer (see https://doi.org/10.48550/arXiv.2204.09359)
+Nw = 300;
+Nts = 1;
+
+% set sampling time
+Ts = 1e-1;
+
+% set initial and final time instant
+t0 = 0;
+% tend = 4;
+% uncomment to test the MHE with a single optimisation step
+% [Y,T] = step(sys_P);
+% tend = T(end);
+if Nts > 1
+    tend = Ts*(Nw*Nts);
+else
+    tend = Ts*(Nw*Nts-1);
+end
+% tend = 5;
+
+%%%% params init function %%%%
+% function: this function shall be in the following form:
+% params = params_model()
+% INPUT: no input
+% OUTPUT: 
+% params: structure with all the necessary parameter to implement the
+% model equations. e.g. for a mechanical system
+% params.M = mass
+% params.b = friction coefficient
+params_init = @params_TCV_Zaccarian;
+
+%%%% params update function %%%%
+% remark: this file is used if the MHE is set to estimate mode parameters
+% too. 
+% function: this file shall be in the following form:
+% params = params_update(params,x)
+% INPUT: 
+% params: structure with all the model parameters (see @params_model)
+% x: state vector
+% OUTPUT: 
+% params_out: updated structure with the new model parameters 
+params_update = @params_update_TCV_Zaccarian;
+
+
+%%%% model function %%%%
+% function: this file shall be in the following form:
+% xdot = model_function(t,x,params,obs) 
+% INPUT:
+% t: time instant (used by integration like ode45)
+% x: current state
+% params: structure with model parameters (see params_init)
+% obs: instance of the obsopt observer class
+% OUTPUT:
+% xdot:output of the state space model
+model = @model_TCV_reference_Zaccarian_outAll;
+
+%%%% model reference function %%%%
+% remark: !DEVEL! this function is used to generate the reference
+% trajectory when the MHE is used in control design. Default considers no
+% dynamics, namely simple output tracking. If an MRAC is considered then
+% @model should be used.
+% function: this file shall be in the same form as @model_function
+% INPUT:
+% t: time instant (used by integration like ode45)
+% x: current state
+% params: structure with model parameters (see params_init)
+% obs: instance of the obsopt observer class
+% OUTPUT:
+% xdot:output of the state space model
+% model_reference = model;
+model_reference = @model_TCV_reference_Zaccarian_outAll;
+
+%%%% measure function %%%%
+% function: this file shall be in the following form:   
+% y = measure_function(x,params,t)
+% INPUT: 
+% x = current state
+% params = structure with model parameters (see params_init)
+% t: time instant
+% OUTPUT:
+% y = measure (no noise added). In the following examples it holds
+% y = x(params.observed_state) (see params_init options)
+measure = @measure_TCV_reference;
+measure_reference = @measure_TCV_reference;
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%%% filters %%%%
+% remark: this function defines the output filtering applied on the
+% measurements. This is fundamental for the filtered MHE (see 
+% https://doi.org/10.48550/arXiv.2204.09359)
+% function: this file shall be in the following form:   
+% [filter, filterScale] = filter_define(Ts,Nts)
+% INPUT: 
+% Ts: sampling time
+% Nts: down-sampling (see https://doi.org/10.48550/arXiv.2204.09359)
+% OUTPUT:
+% filter: structure with the filter transfer function and ss realization
+% filterScale: array weighting the filters in the cost function
+[filter, filterScale] = filter_define(Ts,Nts);
+
+%%%% integration method %%%%
+% ode45-like integration method. For discrete time systems use @odeDD
+ode = @oderk4_fast;
+
+
+%%%% input law %%%
+% function: defines the input law used. Remember to check the @model
+% function to correctly use the inputs. 
+% INPUT:
+% t: time instant
+% drive: error variable
+% params: structure with model parameters (see params_init)
+% OUTPUT:
+% u: control variable
+input_law = @control_TCV;
+
+%%%% measurement noise %%%%
+% this should be a vector with 2 columns and as many rows as the state
+% dimension. All the noise are considered as Gaussian distributed. The 
+% first column defines the mean while the second column the variance.
+% noise_mat = 0*ones(size(sys_P.C,1),2);
+% noise_mat(1:2,2) = 5e-1;
+noise_mat = 0*ones(1,2); % Zaccarian
+
+%%%% params init %%%%
+% init the parameters structure through funtion @model_init. 
+% The model_init file has lots of setup options (varargin). The most 
+% important is the 'params_init' option, which takes as input the function 
+% handle to the previously defined @params_init. For more information see 
+% directly the model_init.m file.
+params = model_init('Ts',Ts,'T0',[t0, tend],'noise',0, 'params_update', params_update, ...
+            'model',model,'measure',measure,'ode',ode, 'odeset', [1e-3 1e-6], ...
+            'input_enable',1,'input_law',input_law,'params_init',params_init);
+             
+%%%% observer init %%%%
+% defien arrival cost
+terminal_states = params.opt_vars;
+terminal_weights = 1e-2*ones(size(terminal_states));
+% terminal_weights(3:end) = 1e-1;
+
+% create observer class instance. For more information on the setup
+% options check directly the class constructor in obsopt.m
+obs = obsopt('DataType', 'simulated', 'optimise', 1, 'GlobalSearch', 0, 'MultiStart', 0, 'J_normalise', 1, 'MaxOptTime', Inf, ... 
+          'Nw', Nw, 'Nts', Nts, 'ode', ode, 'PE_maxiter', 0, 'WaitAllBuffer', 1, 'params',params, 'filters', filterScale,'filterTF', filter, ...
+          'model_reference',model_reference, 'measure_reference',measure_reference, ...
+          'Jdot_thresh',0.95,'MaxIter', 100, 'Jterm_store', 1, 'AlwaysOpt', 1 , 'print', 1 , 'SafetyDensity', 2, 'AdaptiveHist', [5e-3, 2.5e-2, 1e0], ...
+          'AdaptiveSampling',0, 'FlushBuffer', 1, 'opt', @fminsearch, 'terminal', 0, 'terminal_states', terminal_states, 'terminal_weights', terminal_weights, 'terminal_normalise', 1, ...
+          'ConPos', [], 'LBcon', [], 'UBcon', [], 'Bounds', 0);
+
+%% %%%% SIMULATION %%%%
+% remark: the obs.setup.Ntraj variable describes on how many different
+% trajectories the MHE is run. This makes sense in the control design
+% framework, which is under development. If standard MHE is to be used,
+% please ignore obs.setup.Ntraj as it is set to 1.
+
+% start time counter
+t0 = tic;
+
+% integration loop
+for i = 1:obs.setup.Niter
+    
+    % Display iteration step
+    if ((mod(i,10) == 0) || (i == 1))
+        clc
+        disp(['Iteration Number: ', num2str(obs.setup.time(i)),'/',num2str(obs.setup.time(obs.setup.Niter))])
+        disp(['Last J:', num2str(obs.init.Jstory(end))]);
+    end
+    
+    % set current iteration in the obsopt class
+    obs.init.ActualTimeIndex = i;
+    obs.init.t = obs.setup.time(i);
+    
+    %%%% PROPAGATION %%%%
+    % forward propagation of the previous estimate
+    for traj = 1:obs.setup.Ntraj
+        
+        % update traj
+        obs.init.traj = traj;
+        obs.init.params.traj = traj;
+        obs.setup.params.traj = traj;
+                 
+        % propagate only if the time gets over the initial time instant
+        if(obs.init.ActualTimeIndex > 1)
+            
+            % define the time span of the integration
+            startpos = obs.init.ActualTimeIndex-1;
+            stoppos = obs.init.ActualTimeIndex;
+            tspan = obs.setup.time(startpos:stoppos);   
+            
+            % true system - correct initial condition and no noise
+            % considered            
+            X = obs.setup.ode(@(t,x)obs.setup.model_reference(t, x, obs.setup.params, obs), tspan, obs.init.X(traj).val(:,startpos),params.odeset); 
+            obs.init.X(traj).val(:,startpos:stoppos) = [X.y(:,1),X.y(:,end)];
+
+            % real system - initial condition perturbed 
+            X = obs.setup.ode(@(t,x)obs.setup.model(t, x, obs.init.params, obs), tspan, obs.init.X_est(traj).val(:,startpos),params.odeset);
+            obs.init.X_est(traj).val(:,startpos:stoppos) = [X.y(:,1),X.y(:,end)]; 
+                      
+        end
+        
+        %%%% REAL MEASUREMENT %%%%
+        % here the noise is noise added aggording to noise_spec
+        obs.init.Ytrue_full_story(traj).val(1,:,obs.init.ActualTimeIndex) = obs.setup.measure_reference(obs.init.X(traj).val(:,obs.init.ActualTimeIndex),obs.init.params,obs.setup.time(obs.init.ActualTimeIndex),...
+                                                                            obs.init.input_story_ref(traj).val(:,max(1,obs.init.ActualTimeIndex-1)));
+        obs.init.Yref_full_story(traj).val(1,:,obs.init.ActualTimeIndex) = obs.setup.measure_reference(obs.init.X(traj).val(:,obs.init.ActualTimeIndex),obs.init.params,obs.setup.time(obs.init.ActualTimeIndex),...
+                                                                            obs.init.input_story_ref(traj).val(:,max(1,obs.init.ActualTimeIndex-1)));
+        obs.init.noise_story(traj).val(:,obs.init.ActualTimeIndex) = zeros(obs.setup.dim_out,1);
+        y_meas(traj).val =  reshape(obs.init.Yref_full_story(traj).val(1,:,obs.init.ActualTimeIndex),obs.setup.dim_out,1) + obs.init.noise_story(traj).val(:,obs.init.ActualTimeIndex);
+    end
+      
+    %%%% OBSERVER %%%%
+    % start time counter for the observation
+    t1 = tic;
+    
+    % call the observer
+    obs = obs.observer(obs.init.X_est,y_meas);
+   
+    % stop time counter for the observer. Each optimisation process is
+    % timed.
+    obs.init.iter_time(obs.init.ActualTimeIndex) = toc(t1);
+
+end
+
+%%%% SNR %%%
+% the SNR is computed on the mesurements https://ieeexplore.ieee.org/document/9805818 
+for traj = 1:obs.setup.Ntraj
+    for i=1:obs.setup.dim_out
+        obs.init.SNR(traj).val(i) = 10*log(sum(obs.init.Ytrue_full_story(traj).val(1,i,:).^2)/sum(obs.init.noise_story(traj).val(i,:).^2));
+    end
+end
+
+% overall computation time
+obs.init.total_time = toc(t0);
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%% %%%%%%%%%%%%%%%%%%%%%%%%%%%% PLOTS %%%%%%%%%%%%%%%%%%%%%
+% obs self plots
+% obs.plot_section_control(); 
+
+% the whole process could be long, why not going for a nap? No worries, 
+% this "sounds" like a nice way to wake up. (Uncomment)
+% load handel
+% sound(y,Fs)
+end
+
diff --git a/Lib/simulation_scripts/TCV/simulation_TCV_Lsim.m b/Lib/simulation_scripts/TCV/simulation_TCV_Lsim.m
new file mode 100644
index 0000000..5bfdd7d
--- /dev/null
+++ b/Lib/simulation_scripts/TCV/simulation_TCV_Lsim.m
@@ -0,0 +1,264 @@
+%% SIMULATION_GENERAL_V3
+% file: simulation_general_v3.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: function to setup and use the MHE observer on general model
+% INPUT: none
+% OUTPUT: params,obs
+function [obs, params] = simulation_TCV_Lsim
+
+%%%% Init Section %%%%
+% uncomment to close previously opened figures
+% close all
+
+% rng('default');
+rng(1);
+    
+% init observer buffer (see https://doi.org/10.48550/arXiv.2204.09359)
+Nw = 3000;
+Nts = 1e0;
+
+% set sampling time
+Ts = 1e-1;
+
+% set initial and final time instant
+t0 = 0;
+% tend = 4;
+% uncomment to test the MHE with a single optimisation step
+% [Y,T] = step(sys_P);
+% tend = T(end);
+if Nts > 1
+    tend = Ts*(Nw*Nts);
+else
+    tend = Ts*(Nw*Nts-1);
+end
+% tend = 5;
+
+%%%% params init function %%%%
+% function: this function shall be in the following form:
+% params = params_model()
+% INPUT: no input
+% OUTPUT: 
+% params: structure with all the necessary parameter to implement the
+% model equations. e.g. for a mechanical system
+% params.M = mass
+% params.b = friction coefficient
+params_init = @params_TCV_Zaccarian;
+
+%%%% params update function %%%%
+% remark: this file is used if the MHE is set to estimate mode parameters
+% too. 
+% function: this file shall be in the following form:
+% params = params_update(params,x)
+% INPUT: 
+% params: structure with all the model parameters (see @params_model)
+% x: state vector
+% OUTPUT: 
+% params_out: updated structure with the new model parameters 
+params_update = @params_update_TCV_Zaccarian;
+
+
+%%%% model function %%%%
+% function: this file shall be in the following form:
+% xdot = model_function(t,x,params,obs) 
+% INPUT:
+% t: time instant (used by integration like ode45)
+% x: current state
+% params: structure with model parameters (see params_init)
+% obs: instance of the obsopt observer class
+% OUTPUT:
+% xdot:output of the state space model
+model = @model_TCV_Zaccarian_Lsim;
+
+%%%% model reference function %%%%
+% remark: !DEVEL! this function is used to generate the reference
+% trajectory when the MHE is used in control design. Default considers no
+% dynamics, namely simple output tracking. If an MRAC is considered then
+% @model should be used.
+% function: this file shall be in the same form as @model_function
+% INPUT:
+% t: time instant (used by integration like ode45)
+% x: current state
+% params: structure with model parameters (see params_init)
+% obs: instance of the obsopt observer class
+% OUTPUT:
+% xdot:output of the state space model
+% model_reference = model;
+model_reference = @model_TCV_reference_Zaccarian_outAll_Lsim;
+
+%%%% measure function %%%%
+% function: this file shall be in the following form:   
+% y = measure_function(x,params,t)
+% INPUT: 
+% x = current state
+% params = structure with model parameters (see params_init)
+% t: time instant
+% OUTPUT:
+% y = measure (no noise added). In the following examples it holds
+% y = x(params.observed_state) (see params_init options)
+measure = @measure_TCV_reference;
+measure_reference = @measure_TCV_reference;
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%%% filters %%%%
+% remark: this function defines the output filtering applied on the
+% measurements. This is fundamental for the filtered MHE (see 
+% https://doi.org/10.48550/arXiv.2204.09359)
+% function: this file shall be in the following form:   
+% [filter, filterScale] = filter_define(Ts,Nts)
+% INPUT: 
+% Ts: sampling time
+% Nts: down-sampling (see https://doi.org/10.48550/arXiv.2204.09359)
+% OUTPUT:
+% filter: structure with the filter transfer function and ss realization
+% filterScale: array weighting the filters in the cost function
+[filter, filterScale] = filter_define(Ts,Nts);
+
+%%%% integration method %%%%
+% ode45-like integration method. For discrete time systems use @odeDD
+ode = @odeLsim;
+
+
+%%%% input law %%%
+% function: defines the input law used. Remember to check the @model
+% function to correctly use the inputs. 
+% INPUT:
+% t: time instant
+% drive: error variable
+% params: structure with model parameters (see params_init)
+% OUTPUT:
+% u: control variable
+input_law = @control_TCV;
+
+
+%%%% measurement noise %%%%
+% this should be a vector with 2 columns and as many rows as the state
+% dimension. All the noise are considered as Gaussian distributed. The 
+% first column defines the mean while the second column the variance.
+% noise_mat = 0*ones(size(sys_P.C,1),2);
+% noise_mat(1:2,2) = 5e-1;
+noise_mat = 0*ones(1,2); % Zaccarian
+
+%%%% params init %%%%
+% init the parameters structure through funtion @model_init. 
+% The model_init file has lots of setup options (varargin). The most 
+% important is the 'params_init' option, which takes as input the function 
+% handle to the previously defined @params_init. For more information see 
+% directly the model_init.m file.
+params = model_init('Ts',Ts,'T0',[t0, tend],'noise',0, 'params_update', params_update, ...
+            'model',model,'measure',measure,'ode',ode, 'odeset', [1e-3 1e-6], ...
+            'input_enable',1,'input_law',input_law,'params_init',params_init);
+             
+%%%% observer init %%%%
+% defien arrival cost
+terminal_states = params.opt_vars;
+terminal_weights = 1e-2*ones(size(terminal_states));
+% terminal_weights(3:end) = 1e-1;
+
+% create observer class instance. For more information on the setup
+% options check directly the class constructor in obsopt.m
+obs = obsopt('DataType', 'simulated', 'optimise', 1, 'MultiStart', 0, 'J_normalise', 1, 'MaxOptTime', Inf, ... 
+          'Nw', Nw, 'Nts', Nts, 'ode', ode, 'PE_maxiter', 0, 'WaitAllBuffer', 1, 'params',params, 'filters', filterScale,'filterTF', filter, ...
+          'model_reference',model_reference, 'measure_reference',measure_reference, ...
+          'Jdot_thresh',0.95,'MaxIter', 20, 'Jterm_store', 1, 'AlwaysOpt', 1 , 'print', 1 , 'SafetyDensity', 2, 'AdaptiveHist', [5e-3, 2.5e-2, 1e0], ...
+          'AdaptiveSampling',0, 'FlushBuffer', 1, 'opt', @fminsearchcon, 'terminal', 0, 'terminal_states', terminal_states, 'terminal_weights', terminal_weights, 'terminal_normalise', 1, ...
+          'ConPos', [], 'LBcon', [], 'UBcon', [], 'NONCOLcon', @nonlcon_fcn,'Bounds', 0);
+
+%% %%%% SIMULATION %%%%
+% remark: the obs.setup.Ntraj variable describes on how many different
+% trajectories the MHE is run. This makes sense in the control design
+% framework, which is under development. If standard MHE is to be used,
+% please ignore obs.setup.Ntraj as it is set to 1.
+
+% start time counter
+t0 = tic;
+
+% integration loop
+           
+    %%%% PROPAGATION %%%%
+    % forward propagation of the previous estimate
+    disp('Integrating One Shot')
+    for traj = 1:obs.setup.Ntraj
+        
+        % update traj
+        obs.init.traj = traj;
+        obs.init.params.traj = traj;
+        obs.setup.params.traj = traj;
+                 
+        % propagate 
+        tspan = obs.setup.time;   
+
+        % true system - correct initial condition and no noise
+        % considered            
+        X = obs.setup.ode(@(t,x)obs.setup.model_reference(t, x, obs.setup.params, obs), tspan, obs.init.X(traj).val(:,1),params.odeset); 
+        obs.init.X(traj).val = X.y;
+
+        % real system - initial condition perturbed 
+        X = obs.setup.ode(@(t,x)obs.setup.model(t, x, obs.init.params, obs), tspan, obs.init.X_est(traj).val(:,1),params.odeset);
+        obs.init.X_est(traj).val = X.y; 
+        
+    end
+        
+    for i = 1:obs.setup.Niter
+        
+        % Display iteration step
+        if ((mod(i,1000) == 0) || (i == 1))
+            clc
+            disp(['Iteration Number: ', num2str(obs.setup.time(i)),'/',num2str(obs.setup.time(obs.setup.Niter))])
+            disp(['Last J:', num2str(obs.init.Jstory(end))]);
+        end
+
+        % set current iteration in the obsopt class
+        obs.init.ActualTimeIndex = i;
+        obs.init.t = obs.setup.time(i);
+            
+        for traj = 1:obs.setup.Ntraj
+
+            %%%% REAL MEASUREMENT %%%%
+            % here the noise is noise added aggording to noise_spec
+            obs.init.Ytrue_full_story(traj).val(1,:,obs.init.ActualTimeIndex) = obs.setup.measure_reference(obs.init.X(traj).val(:,obs.init.ActualTimeIndex),obs.init.params,obs.setup.time(obs.init.ActualTimeIndex),...
+                                                                                obs.init.input_story_ref(traj).val(:,max(1,obs.init.ActualTimeIndex-1)));
+            obs.init.Yref_full_story(traj).val(1,:,obs.init.ActualTimeIndex) = obs.setup.measure_reference(obs.init.X(traj).val(:,obs.init.ActualTimeIndex),obs.init.params,obs.setup.time(obs.init.ActualTimeIndex),...
+                                                                                obs.init.input_story_ref(traj).val(:,max(1,obs.init.ActualTimeIndex-1)));
+            obs.init.noise_story(traj).val(:,obs.init.ActualTimeIndex) = zeros(obs.setup.dim_out,1);
+            y_meas(traj).val =  reshape(obs.init.Yref_full_story(traj).val(1,:,obs.init.ActualTimeIndex),obs.setup.dim_out,1) + obs.init.noise_story(traj).val(:,obs.init.ActualTimeIndex);
+           
+        end                
+        
+         
+        %%%% OBSERVER %%%%
+        % start time counter for the observation
+        t1 = tic;
+
+        % call the observer
+        obs = obs.observer(obs.init.X_est,y_meas);
+
+        % stop time counter for the observer. Each optimisation process is
+        % timed.
+        obs.init.iter_time(obs.init.ActualTimeIndex) = toc(t1);
+        
+    end
+        
+              
+%%%% SNR %%%
+% the SNR is computed on the mesurements https://ieeexplore.ieee.org/document/9805818 
+for traj = 1:obs.setup.Ntraj
+    for i=1:obs.setup.dim_out
+        obs.init.SNR(traj).val(i) = 10*log(sum(obs.init.Ytrue_full_story(traj).val(1,i,:).^2)/sum(obs.init.noise_story(traj).val(i,:).^2));
+    end
+end
+
+% overall computation time
+obs.init.total_time = toc(t0);
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%% %%%%%%%%%%%%%%%%%%%%%%%%%%%% PLOTS %%%%%%%%%%%%%%%%%%%%%
+% obs self plots
+% obs.plot_section_control(); 
+
+% the whole process could be long, why not going for a nap? No worries, 
+% this "sounds" like a nice way to wake up. (Uncomment)
+% load handel
+% sound(y,Fs)
+end
+
diff --git a/Lib/simulation_scripts/TCV/simulation_TCV_Nonlinear.m b/Lib/simulation_scripts/TCV/simulation_TCV_Nonlinear.m
new file mode 100644
index 0000000..371ab91
--- /dev/null
+++ b/Lib/simulation_scripts/TCV/simulation_TCV_Nonlinear.m
@@ -0,0 +1,264 @@
+%% SIMULATION_GENERAL_V3
+% file: simulation_general_v3.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: function to setup and use the MHE observer on general model
+% INPUT: none
+% OUTPUT: params,obs
+function [obs, params] = simulation_TCV_Nonlinear
+
+%%%% Init Section %%%%
+% uncomment to close previously opened figures
+% close all
+
+% rng('default');
+rng(1);
+    
+% init observer buffer (see https://doi.org/10.48550/arXiv.2204.09359)
+Nw = 300;
+Nts = 1e0;
+
+% set sampling time
+Ts = 1e-1;
+
+% set initial and final time instant
+t0 = 0;
+% tend = 4;
+% uncomment to test the MHE with a single optimisation step
+% [Y,T] = step(sys_P);
+% tend = T(end);
+if Nts > 1
+    tend = Ts*(Nw*Nts);
+else
+    tend = Ts*(Nw*Nts-1);
+end
+% tend = 5;
+
+%%%% params init function %%%%
+% function: this function shall be in the following form:
+% params = params_model()
+% INPUT: no input
+% OUTPUT: 
+% params: structure with all the necessary parameter to implement the
+% model equations. e.g. for a mechanical system
+% params.M = mass
+% params.b = friction coefficient
+params_init = @params_TCV_Zaccarian_Nonlinear;
+
+%%%% params update function %%%%
+% remark: this file is used if the MHE is set to estimate mode parameters
+% too. 
+% function: this file shall be in the following form:
+% params = params_update(params,x)
+% INPUT: 
+% params: structure with all the model parameters (see @params_model)
+% x: state vector
+% OUTPUT: 
+% params_out: updated structure with the new model parameters 
+params_update = @params_update_TCV_Zaccarian_Nonlinear;
+
+
+%%%% model function %%%%
+% function: this file shall be in the following form:
+% xdot = model_function(t,x,params,obs) 
+% INPUT:
+% t: time instant (used by integration like ode45)
+% x: current state
+% params: structure with model parameters (see params_init)
+% obs: instance of the obsopt observer class
+% OUTPUT:
+% xdot:output of the state space model
+model = @model_TCV_Zaccarian_Nonlinear;
+
+%%%% model reference function %%%%
+% remark: !DEVEL! this function is used to generate the reference
+% trajectory when the MHE is used in control design. Default considers no
+% dynamics, namely simple output tracking. If an MRAC is considered then
+% @model should be used.
+% function: this file shall be in the same form as @model_function
+% INPUT:
+% t: time instant (used by integration like ode45)
+% x: current state
+% params: structure with model parameters (see params_init)
+% obs: instance of the obsopt observer class
+% OUTPUT:
+% xdot:output of the state space model
+% model_reference = model;
+model_reference = @model_TCV_reference_Zaccarian_outAll;
+
+%%%% measure function %%%%
+% function: this file shall be in the following form:   
+% y = measure_function(x,params,t)
+% INPUT: 
+% x = current state
+% params = structure with model parameters (see params_init)
+% t: time instant
+% OUTPUT:
+% y = measure (no noise added). In the following examples it holds
+% y = x(params.observed_state) (see params_init options)
+measure = @measure_TCV_reference;
+measure_reference = @measure_TCV_reference;
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%%% filters %%%%
+% remark: this function defines the output filtering applied on the
+% measurements. This is fundamental for the filtered MHE (see 
+% https://doi.org/10.48550/arXiv.2204.09359)
+% function: this file shall be in the following form:   
+% [filter, filterScale] = filter_define(Ts,Nts)
+% INPUT: 
+% Ts: sampling time
+% Nts: down-sampling (see https://doi.org/10.48550/arXiv.2204.09359)
+% OUTPUT:
+% filter: structure with the filter transfer function and ss realization
+% filterScale: array weighting the filters in the cost function
+[filter, filterScale] = filter_define(Ts,Nts);
+
+%%%% integration method %%%%
+% ode45-like integration method. For discrete time systems use @odeDD
+ode = @odeEuler;
+
+
+%%%% input law %%%
+% function: defines the input law used. Remember to check the @model
+% function to correctly use the inputs. 
+% INPUT:
+% t: time instant
+% drive: error variable
+% params: structure with model parameters (see params_init)
+% OUTPUT:
+% u: control variable
+input_law = @control;
+
+
+%%%% measurement noise %%%%
+% this should be a vector with 2 columns and as many rows as the state
+% dimension. All the noise are considered as Gaussian distributed. The 
+% first column defines the mean while the second column the variance.
+% noise_mat = 0*ones(size(sys_P.C,1),2);
+% noise_mat(1:2,2) = 5e-1;
+noise_mat = 0*ones(1,2); % Zaccarian
+
+%%%% params init %%%%
+% init the parameters structure through funtion @model_init. 
+% The model_init file has lots of setup options (varargin). The most 
+% important is the 'params_init' option, which takes as input the function 
+% handle to the previously defined @params_init. For more information see 
+% directly the model_init.m file.
+params = model_init('Ts',Ts,'T0',[t0, tend],'noise',0, 'params_update', params_update, ...
+            'model',model,'measure',measure,'ode',ode, 'odeset', [1e-3 1e-6], ...
+            'input_enable',1,'input_law',input_law,'params_init',params_init);
+             
+%%%% observer init %%%%
+% defien arrival cost
+terminal_states = params.opt_vars;
+terminal_weights = 1e-2*ones(size(terminal_states));
+% terminal_weights(3:end) = 1e-1;
+
+% create observer class instance. For more information on the setup
+% options check directly the class constructor in obsopt.m
+obs = obsopt('DataType', 'simulated', 'optimise', 1, 'MultiStart', 0, 'J_normalise', 1, 'MaxOptTime', Inf, ... 
+          'Nw', Nw, 'Nts', Nts, 'ode', ode, 'PE_maxiter', 0, 'WaitAllBuffer', 1, 'params',params, 'filters', filterScale,'filterTF', filter, ...
+          'model_reference',model_reference, 'measure_reference',measure_reference, ...
+          'Jdot_thresh',0.95,'MaxIter', 20, 'Jterm_store', 1, 'AlwaysOpt', 1 , 'print', 1 , 'SafetyDensity', 2, 'AdaptiveHist', [5e-3, 2.5e-2, 1e0], ...
+          'AdaptiveSampling',0, 'FlushBuffer', 1, 'opt', @patternsearch, 'terminal', 0, 'terminal_states', terminal_states, 'terminal_weights', terminal_weights, 'terminal_normalise', 1, ...
+          'ConPos', [], 'LBcon', [], 'UBcon', [], 'NONCOLcon', @nonlcon_fcn,'Bounds', 0);
+
+%% %%%% SIMULATION %%%%
+% remark: the obs.setup.Ntraj variable describes on how many different
+% trajectories the MHE is run. This makes sense in the control design
+% framework, which is under development. If standard MHE is to be used,
+% please ignore obs.setup.Ntraj as it is set to 1.
+
+% start time counter
+t0 = tic;
+
+% integration loop
+           
+    %%%% PROPAGATION %%%%
+    % forward propagation of the previous estimate
+    disp('Integrating One Shot')
+    for traj = 1:obs.setup.Ntraj
+        
+        % update traj
+        obs.init.traj = traj;
+        obs.init.params.traj = traj;
+        obs.setup.params.traj = traj;
+                 
+        % propagate 
+        tspan = obs.setup.time;   
+
+        % true system - correct initial condition and no noise
+        % considered            
+        X = obs.setup.ode(@(t,x)obs.setup.model_reference(t, x, obs.setup.params, obs), tspan, obs.init.X(traj).val(:,1),params.odeset); 
+        obs.init.X(traj).val = X.y;
+
+        % real system - initial condition perturbed 
+        X = obs.setup.ode(@(t,x)obs.setup.model(t, x, obs.init.params, obs), tspan, obs.init.X_est(traj).val(:,1),params.odeset);
+        obs.init.X_est(traj).val = X.y; 
+        
+    end
+        
+    for i = 1:obs.setup.Niter
+        
+        % Display iteration step
+        if ((mod(i,1000) == 0) || (i == 1))
+            clc
+            disp(['Iteration Number: ', num2str(obs.setup.time(i)),'/',num2str(obs.setup.time(obs.setup.Niter))])
+            disp(['Last J:', num2str(obs.init.Jstory(end))]);
+        end
+
+        % set current iteration in the obsopt class
+        obs.init.ActualTimeIndex = i;
+        obs.init.t = obs.setup.time(i);
+            
+        for traj = 1:obs.setup.Ntraj
+
+            %%%% REAL MEASUREMENT %%%%
+            % here the noise is noise added aggording to noise_spec
+            obs.init.Ytrue_full_story(traj).val(1,:,obs.init.ActualTimeIndex) = obs.setup.measure_reference(obs.init.X(traj).val(:,obs.init.ActualTimeIndex),obs.init.params,obs.setup.time(obs.init.ActualTimeIndex),...
+                                                                                obs.init.input_story_ref(traj).val(:,max(1,obs.init.ActualTimeIndex-1)));
+            obs.init.Yref_full_story(traj).val(1,:,obs.init.ActualTimeIndex) = obs.setup.measure_reference(obs.init.X(traj).val(:,obs.init.ActualTimeIndex),obs.init.params,obs.setup.time(obs.init.ActualTimeIndex),...
+                                                                                obs.init.input_story_ref(traj).val(:,max(1,obs.init.ActualTimeIndex-1)));
+            obs.init.noise_story(traj).val(:,obs.init.ActualTimeIndex) = zeros(obs.setup.dim_out,1);
+            y_meas(traj).val =  reshape(obs.init.Yref_full_story(traj).val(1,:,obs.init.ActualTimeIndex),obs.setup.dim_out,1) + obs.init.noise_story(traj).val(:,obs.init.ActualTimeIndex);
+           
+        end                
+        
+         
+        %%%% OBSERVER %%%%
+        % start time counter for the observation
+        t1 = tic;
+
+        % call the observer
+        obs = obs.observer(obs.init.X_est,y_meas);
+
+        % stop time counter for the observer. Each optimisation process is
+        % timed.
+        obs.init.iter_time(obs.init.ActualTimeIndex) = toc(t1);
+        
+    end
+        
+              
+%%%% SNR %%%
+% the SNR is computed on the mesurements https://ieeexplore.ieee.org/document/9805818 
+for traj = 1:obs.setup.Ntraj
+    for i=1:obs.setup.dim_out
+        obs.init.SNR(traj).val(i) = 10*log(sum(obs.init.Ytrue_full_story(traj).val(1,i,:).^2)/sum(obs.init.noise_story(traj).val(i,:).^2));
+    end
+end
+
+% overall computation time
+obs.init.total_time = toc(t0);
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%% %%%%%%%%%%%%%%%%%%%%%%%%%%%% PLOTS %%%%%%%%%%%%%%%%%%%%%
+% obs self plots
+% obs.plot_section_control(); 
+
+% the whole process could be long, why not going for a nap? No worries, 
+% this "sounds" like a nice way to wake up. (Uncomment)
+% load handel
+% sound(y,Fs)
+end
+
diff --git a/Lib/simulation_scripts/TCV/simulation_TCV_discrete.m b/Lib/simulation_scripts/TCV/simulation_TCV_discrete.m
new file mode 100644
index 0000000..cde2c0c
--- /dev/null
+++ b/Lib/simulation_scripts/TCV/simulation_TCV_discrete.m
@@ -0,0 +1,258 @@
+%% SIMULATION_GENERAL_V3
+% file: simulation_general_v3.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: function to setup and use the MHE observer on general model
+% INPUT: none
+% OUTPUT: params,obs
+function [params,obs] = simulation_TCV_discrete(dim,sys_P,staticAll,discrete)
+
+%%%% Init Section %%%%
+% uncomment to close previously opened figures
+% close all
+
+rng('default');
+    
+% init observer buffer (see https://doi.org/10.48550/arXiv.2204.09359)
+Nw =100;
+Nts = 5;
+
+% set sampling time
+Ts = 5e-2;
+
+% set initial and final time instant
+t0 = 0;
+% tend = 4;
+% uncomment to test the MHE with a single optimisation step
+% [Y,T] = step(sys_P);
+% tend = T(end);
+tend = (Nw*Nts-1)*Ts;
+% tend = 5;
+
+%%%% params init function %%%%
+% function: this function shall be in the following form:
+% params = params_model()
+% INPUT: no input
+% OUTPUT: 
+% params: structure with all the necessary parameter to implement the
+% model equations. e.g. for a mechanical system
+% params.M = mass
+% params.b = friction coefficient
+params_init = @params_TCV_Zaccarian_outAll;
+
+%%%% params update function %%%%
+% remark: this file is used if the MHE is set to estimate mode parameters
+% too. 
+% function: this file shall be in the following form:
+% params = params_update(params,x)
+% INPUT: 
+% params: structure with all the model parameters (see @params_model)
+% x: state vector
+% OUTPUT: 
+% params_out: updated structure with the new model parameters 
+params_update = @params_update_TCV_Zaccarian_outAll;
+
+
+%%%% model function %%%%
+% function: this file shall be in the following form:
+% xdot = model_function(t,x,params,obs) 
+% INPUT:
+% t: time instant (used by integration like ode45)
+% x: current state
+% params: structure with model parameters (see params_init)
+% obs: instance of the obsopt observer class
+% OUTPUT:
+% xdot:output of the state space model
+model = @model_TCV_Zaccarian_outAll;
+
+%%%% model reference function %%%%
+% remark: !DEVEL! this function is used to generate the reference
+% trajectory when the MHE is used in control design. Default considers no
+% dynamics, namely simple output tracking. If an MRAC is considered then
+% @model should be used.
+% function: this file shall be in the same form as @model_function
+% INPUT:
+% t: time instant (used by integration like ode45)
+% x: current state
+% params: structure with model parameters (see params_init)
+% obs: instance of the obsopt observer class
+% OUTPUT:
+% xdot:output of the state space model
+% model_reference = @model_reference;
+model_reference = @model_TCV_reference_Zaccarian_outAll;
+
+%%%% measure function %%%%
+% function: this file shall be in the following form:   
+% y = measure_function(x,params,t)
+% INPUT: 
+% x = current state
+% params = structure with model parameters (see params_init)
+% t: time instant
+% OUTPUT:
+% y = measure (no noise added). In the following examples it holds
+% y = x(params.observed_state) (see params_init options)
+measure = @measure_TCV_reference;
+measure_reference = @measure_TCV_reference;
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%%% filters %%%%
+% remark: this function defines the output filtering applied on the
+% measurements. This is fundamental for the filtered MHE (see 
+% https://doi.org/10.48550/arXiv.2204.09359)
+% function: this file shall be in the following form:   
+% [filter, filterScale] = filter_define(Ts,Nts)
+% INPUT: 
+% Ts: sampling time
+% Nts: down-sampling (see https://doi.org/10.48550/arXiv.2204.09359)
+% OUTPUT:
+% filter: structure with the filter transfer function and ss realization
+% filterScale: array weighting the filters in the cost function
+[filter, filterScale] = filter_define(Ts,Nts);
+
+%%%% integration method %%%%
+% ode45-like integration method. For discrete time systems use @odeDD
+if discrete == 0
+    ode = @oderk4_fast;
+else
+    ode = @odeDD;
+end
+
+
+%%%% input law %%%
+% function: defines the input law used. Remember to check the @model
+% function to correctly use the inputs. 
+% INPUT:
+% t: time instant
+% drive: error variable
+% params: structure with model parameters (see params_init)
+% OUTPUT:
+% u: control variable
+input_law = @control_TCV;
+
+%%%% measurement noise %%%%
+% this should be a vector with 2 columns and as many rows as the state
+% dimension. All the noise are considered as Gaussian distributed. The 
+% first column defines the mean while the second column the variance.
+% noise_mat = 0*ones(size(sys_P.C,1),2);
+% noise_mat(1:2,2) = 5e-1;
+noise_mat = 0*ones(1,2); % Zaccarian
+
+%%%% params init %%%%
+% init the parameters structure through funtion @model_init. 
+% The model_init file has lots of setup options (varargin). The most 
+% important is the 'params_init' option, which takes as input the function 
+% handle to the previously defined @params_init. For more information see 
+% directly the model_init.m file.
+params = model_init('Ts',Ts,'T0',[t0, tend],'noise',0, 'params_update', params_update, ...
+            'model',model,'measure',measure,'ObservedState',[1 2],'ode',ode, 'odeset', [1e-3 1e-6], ...
+            'input_enable',1,'input_law',input_law,'params_init',params_init,'params_init_vars',{dim,sys_P,staticAll,discrete,Ts});
+             
+%%%% observer init %%%%
+% defien arrival cost
+terminal_states = params.opt_vars;
+terminal_weights = 1e1*ones(size(terminal_states));
+terminal_weights(3:end) = 1e-1;
+
+% create observer class instance. For more information on the setup
+% options check directly the class constructor in obsopt.m
+obs = obsopt('DataType', 'simulated', 'optimise', 0 , 'GlobalSearch', 0, 'MultiStart', 0, 'J_normalise', 1, 'MaxOptTime', Inf, ... 
+          'Nw', Nw, 'Nts', Nts, 'ode', ode, 'PE_maxiter', 0, 'WaitAllBuffer', 1, 'params',params, 'filters', filterScale,'filterTF', filter, ...
+          'model_reference',model_reference, 'measure_reference',measure_reference, ...
+          'Jdot_thresh',0.95,'MaxIter',200, 'Jterm_store', 1, 'AlwaysOpt', 1 , 'print', 1 , 'SafetyDensity', 2, 'AdaptiveHist', [5e-3, 2.5e-2, 1e0], ...
+          'AdaptiveSampling',0, 'FlushBuffer', 1, 'opt', @fminsearch, 'terminal', 0, 'terminal_states', terminal_states, 'terminal_weights', terminal_weights, 'terminal_normalise', 1, ...
+          'ConPos', [], 'LBcon', [], 'UBcon', [], 'Bounds', 0);
+
+%% %%%% SIMULATION %%%%
+% remark: the obs.setup.Ntraj variable describes on how many different
+% trajectories the MHE is run. This makes sense in the control design
+% framework, which is under development. If standard MHE is to be used,
+% please ignore obs.setup.Ntraj as it is set to 1.
+
+% start time counter
+t0 = tic;
+
+% integration loop
+for i = 1:obs.setup.Niter
+    
+    % Display iteration step
+    if ((mod(i,10) == 0) || (i == 1))
+        clc
+        disp(['Iteration Number: ', num2str(obs.setup.time(i)),'/',num2str(obs.setup.time(obs.setup.Niter))])
+        disp(['Last J:', num2str(obs.init.Jstory(end))]);
+    end
+    
+    % set current iteration in the obsopt class
+    obs.init.ActualTimeIndex = i;
+    obs.init.t = obs.setup.time(i);
+    
+    %%%% PROPAGATION %%%%
+    % forward propagation of the previous estimate
+    for traj = 1:obs.setup.Ntraj
+        
+        % update traj
+        obs.init.traj = traj;
+                 
+        % propagate only if the time gets over the initial time instant
+        if(obs.init.ActualTimeIndex > 1)
+            
+            % define the time span of the integration
+            startpos = obs.init.ActualTimeIndex-1;
+            stoppos = obs.init.ActualTimeIndex;
+            tspan = obs.setup.time(startpos:stoppos);   
+            
+            % true system - correct initial condition and no noise
+            % considered            
+            X = obs.setup.ode(@(t,x)obs.setup.model_reference(t, x, obs.setup.params, obs), tspan, obs.init.X(traj).val(:,startpos),params.odeset); 
+            obs.init.X(traj).val(:,startpos:stoppos) = [X.y(:,1),X.y(:,end)];
+
+            % real system - initial condition perturbed 
+            X = obs.setup.ode(@(t,x)obs.setup.model(t, x, obs.init.params, obs), tspan, obs.init.X_est(traj).val(:,startpos),params.odeset);
+            obs.init.X_est(traj).val(:,startpos:stoppos) = [X.y(:,1),X.y(:,end)]; 
+                      
+        end
+        
+        %%%% REAL MEASUREMENT %%%%
+        % here the noise is noise added aggording to noise_spec
+        obs.init.Ytrue_full_story(traj).val(1,:,obs.init.ActualTimeIndex) = obs.setup.measure_reference(obs.init.X(traj).val(:,obs.init.ActualTimeIndex),obs.init.params,obs.setup.time(obs.init.ActualTimeIndex),...
+                                                                            obs.init.input_story_ref(traj).val(:,max(1,obs.init.ActualTimeIndex-1)));
+        obs.init.Yref_full_story(traj).val(1,:,obs.init.ActualTimeIndex) = obs.setup.measure_reference(obs.init.X(traj).val(:,obs.init.ActualTimeIndex),obs.init.params,obs.setup.time(obs.init.ActualTimeIndex),...
+                                                                            obs.init.input_story_ref(traj).val(:,max(1,obs.init.ActualTimeIndex-1)));
+        obs.init.noise_story(traj).val(:,obs.init.ActualTimeIndex) = zeros(length(obs.setup.observed_state),1);
+        y_meas(traj).val =  reshape(obs.init.Yref_full_story(traj).val(1,:,obs.init.ActualTimeIndex),obs.setup.dim_out,1) + obs.init.noise_story(traj).val(:,obs.init.ActualTimeIndex);
+    end
+      
+    %%%% OBSERVER %%%%
+    % start time counter for the observation
+    t1 = tic;
+    
+    % call the observer
+    obs = obs.observer(obs.init.X_est,y_meas);
+   
+    % stop time counter for the observer. Each optimisation process is
+    % timed.
+    obs.init.iter_time(obs.init.ActualTimeIndex) = toc(t1);
+
+end
+
+%%%% SNR %%%
+% the SNR is computed on the mesurements https://ieeexplore.ieee.org/document/9805818 
+for traj = 1:obs.setup.Ntraj
+    for i=1:obs.setup.dim_out
+        obs.init.SNR(traj).val(i) = 10*log(sum(obs.init.Ytrue_full_story(traj).val(1,i,:).^2)/sum(obs.init.noise_story(traj).val(i,:).^2));
+    end
+end
+
+% overall computation time
+obs.init.total_time = toc(t0);
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%% %%%%%%%%%%%%%%%%%%%%%%%%%%%% PLOTS %%%%%%%%%%%%%%%%%%%%%
+% obs self plots
+% obs.plot_section_control(); 
+
+% the whole process could be long, why not going for a nap? No worries, 
+% this "sounds" like a nice way to wake up. (Uncomment)
+% load handel
+% sound(y,Fs)
+end
+
diff --git a/Lib/simulation_scripts/battery/simulation_realdata_battery_EKF.m b/Lib/simulation_scripts/battery/simulation_realdata_battery_EKF.m
new file mode 100644
index 0000000..c5ba88d
--- /dev/null
+++ b/Lib/simulation_scripts/battery/simulation_realdata_battery_EKF.m
@@ -0,0 +1,288 @@
+%% SIMULATION_REALDATA_BATTERY_SIMULINK
+% file: simulation_general_v3.m
+% author: 
+% date: 21/07/2024
+% description: function to setup and use the EKF on battery from
+% real data of CALCE dataset 
+% INPUT: none
+% OUTPUT: ekf_results (struct containing y, z, and time data)
+
+function ekf_results = simulation_realdata_battery_EKF
+
+    clc;
+    close all;
+    clear all;
+
+    rng('default');
+
+    % Loading the dataset
+    params_sim = params_battery_simulink_calce;
+    params = params_battery_calce(params_sim);
+    
+
+    % Covariances and intial SOC
+    % params.initial_soc = 0.4;
+    % P0 = diag([1E-0, 1E-2, 1E-6 *ones(1, 12)]); 
+    % Q = diag([1E-4, 1E-3, 1E-6, 1E-6, 1E-6 *ones(1, 2), 1E-6, 1E-6, 1E-6 *ones(1, 2), 1E-6, 1E-6, 1E-6 *ones(1, 2)]);
+    % R = 1E-6;
+
+    params.initial_soc = 0.4;
+    P0 = diag([1E-2, 1E-2, 1E-6 *ones(1, 12)]); 
+    Q = diag([1E-4, 1E-3, 1E-6*ones(1, 12)]);
+    R = 1E-6;
+    
+    % intermdediate
+    % P0 = diag([1E-2, 1E-2, 1E-3*ones(1, 12)]); 
+    % Q = diag([1E-4, 1E-4, 1E-6*ones(1, 12)]);
+ 
+    % Coefficients
+    Voc_coefficient = [params.alpha_Voc params.beta_Voc params.gamma_Voc params.delta_Voc params.eps_Voc params.xi_Voc];
+    R0_coefficient = [params.alpha_R0 params.beta_R0 params.gamma_R0 params.delta_R0];
+    R1_Coefficient = [params.alpha_R1 params.beta_R1 params.gamma_R1 params.delta_R1];
+    C1_Coefficient = [params.alpha_C1 params.beta_C1 params.gamma_C1 params.delta_C1];
+
+
+    % R0_coefficient = [0.0863 -0.1015 0.1998 -0.1199];
+    % R1_Coefficient = [-0.0020 0.2940 -0.9014 0.7453];
+    % C1_Coefficient = [828.894553983524 9.244656743141175e+03 -3.611686504893436e+04 3.229387762053796e+04];
+    % Battery capacity (converting Ampere-hour to Ampere-second)
+    capacity = params.C_n_h * 3600;
+
+    % Sampling time
+    Ts = params.Ts;
+
+    % Input, output and SOC ground truth
+    u = params.u_sim;
+    y_true = params.y_true_sim;
+    z_true = params.soc_sim;
+    datatime = params.time;
+
+    % Initial states
+    initial_Voc = polyval(flip(Voc_coefficient), params.initial_soc);
+    initial_R0 = polyval(flip(R0_coefficient), params.initial_soc);
+    initial_V1 = initial_Voc - u(1)*initial_R0 - y_true(1);
+    x_initial = [params.initial_soc; initial_V1; R0_coefficient'; R1_Coefficient'; C1_Coefficient'];
+
+    % Initialize the estimated state and prediction
+    x_estimated = zeros(length(x_initial), length(datatime));
+    comp_time = zeros(1, length(datatime)); 
+    y_predict = zeros(1, length(datatime));
+
+    tic; 
+
+    % EKF first step - correction based on initial estimate
+    [~, H] = jacobian(x_initial, u(1), Voc_coefficient, Ts);
+    K = P0 * H' / (H * P0 * H' + R);
+    y_predict(1) = battery_measurement(x_initial, u(1), Voc_coefficient);
+    x_estimated(:, 1) = x_initial + K * (y_true(1) - y_predict(1));
+    P = (eye(length(x_initial)) - K * H) * P0;
+    P = (P + P') / 2; % for symmetry
+
+    comp_time(1) = toc; % Time taken for the first step
+
+    for n = 2:length(datatime)
+        tic;
+
+        % Prediction
+        [F, ~] = jacobian(x_estimated(:, n-1), u(n-1), Voc_coefficient, Ts);
+        P = F * P * F' + Q;
+        P = (P + P') / 2;
+        x_predict = battery_model(x_estimated(:, n-1), u(n-1), Ts, capacity);
+
+        % Correction
+        [~, H] = jacobian(x_predict, u(n), Voc_coefficient, Ts);
+        K = P * H' / (H * P * H' + R);
+        y_predict(n) = battery_measurement(x_predict, u(n), Voc_coefficient);
+        x_estimated(:, n) = x_predict + K * (y_true(n) - y_predict(n));
+        P = (eye(length(x_initial)) - K * H) * P;
+        P = (P + P') / 2;
+
+        comp_time(n) = toc; % Time taken for each step
+    end
+
+    z_estimated = x_estimated(1, :);
+    soc_rmse = sqrt(mean((z_estimated - z_true).^2));
+    voltage_rmse = sqrt(mean((y_predict - y_true).^2));
+    avg_comp_time = mean(comp_time); % Average computation time
+    max_comp_time = max(comp_time); % Maximum computation time
+
+    % Display RMSE and computation times
+    fprintf('SOC RMSE: %.6e\n', soc_rmse);
+    fprintf('Voltage RMSE: %.6e\n', voltage_rmse);
+    fprintf('Mean Computational Time: %.6e seconds\n', avg_comp_time);
+    fprintf('Max Computational Time: %.6e seconds\n', max_comp_time);
+
+    % Assign results to struct
+    ekf_results = struct();
+
+    % SOC Data
+    ekf_results.SOC = struct();
+    ekf_results.SOC.true = z_true;
+    ekf_results.SOC.estimated = z_estimated;
+    ekf_results.SOC.RMSE = soc_rmse;
+
+    % Voltage Data
+    ekf_results.Voltage = struct();
+    ekf_results.Voltage.true = y_true;
+    ekf_results.Voltage.predicted = y_predict;
+    ekf_results.Voltage.RMSE = voltage_rmse;
+
+    % Time Data
+    ekf_results.Time = datatime;
+
+    % Computational Time Data
+    ekf_results.ComputationTime = struct();
+    ekf_results.ComputationTime.average = avg_comp_time;
+    ekf_results.ComputationTime.maximum = max_comp_time;
+
+    % Plot results
+    plot_results(ekf_results);
+
+end
+
+%% Jacobian
+function [F, H] = jacobian(x, u, a_OCV, Ts)
+    z = x(1);
+    V1 = x(2);
+
+    R0_coeff = x(3:6)';
+    R1_coeff = x(7:10)';
+    C1_coeff = x(11:14)';
+
+    soc_poly = [1; z; z^2; z^3];
+    soc_deriv = [0; 1; 2*z; 3*z^2];
+    soc_ocv_poly = [1; z; z^2; z^3; z^4; z^5];
+    soc_ocv_deriv = [0; 1; 2*z; 3*z^2; 4*z^3; 5*z^4];
+
+    R0 = dot(R0_coeff, soc_poly);
+    R1 = dot(R1_coeff, soc_poly);
+    C1 = dot(C1_coeff, soc_poly);
+    tau = R1 * C1;
+
+    R0_deriv = R0_coeff * soc_deriv; 
+    R1_deriv = R1_coeff * soc_deriv; 
+    C1_deriv = C1_coeff * soc_deriv; 
+    tau_deriv = R1_deriv * C1 + R1 * C1_deriv; 
+    OCV_deriv = a_OCV * soc_ocv_deriv; 
+
+
+    % Jacobian for the state transition matrix
+    F = zeros(14, 14);
+
+    F(1, 1) = 1;
+
+    F(2, 1) = V1 * exp(-Ts/tau) * (Ts/tau^2) * tau_deriv ...
+              + u * R1_deriv * (1 - exp(-Ts/tau)) ...
+              - u * R1 * exp(-Ts/tau) * (Ts/tau^2) * tau_deriv;
+    F(2, 2) = exp(-Ts/tau);
+
+    F(2, 3) = 0;
+    F(2, 4) = 0;
+    F(2, 5) = 0;
+    F(2, 6) = 0;
+
+    F(2, 7) = V1 * exp(-Ts/tau) * (Ts/tau^2) * C1 ...
+              + u * (1-exp(-Ts/tau)) ...
+              - u * R1 * exp(-Ts/tau) * (Ts/tau^2) * C1;
+    F(2, 8) = V1 * exp(-Ts/tau) * (Ts/tau^2) * C1 * z ...
+               + u * (1-exp(-Ts/tau)) * z ...
+               - u * R1 * exp(-Ts/tau) * (Ts/tau^2) * C1 * z;
+    F(2, 9) = V1 * exp(-Ts/tau) * (Ts/tau^2) * C1 * z^2 ...
+               + u * (1-exp(-Ts/tau)) * z^2 ...
+               - u * R1 * exp(-Ts/tau) * (Ts/tau^2) * C1 * z^2;
+    F(2, 10) = V1 * exp(-Ts/tau) * (Ts/tau^2) * C1 * z^3 ...
+               + u * (1-exp(-Ts/tau)) * z^3 ...
+               - u * R1 * exp(-Ts/tau) * (Ts/tau^2) * C1 * z^3;
+
+    F(2, 11) = (V1 - u * R1) * (exp(-Ts/tau) * Ts * R1/tau^2);
+    F(2, 12) = (V1 - u * R1) * (exp(-Ts/tau) * Ts * R1 * z/tau^2);
+    F(2, 13) = (V1 - u * R1) * (exp(-Ts/tau) * Ts * R1 * z^2/tau^2);
+    F(2, 14) = (V1 - u * R1) * (exp(-Ts/tau) * Ts * R1 * z^3/tau^2);
+
+    F(3:14, 3:14) = eye(12);
+
+    % Jacobian for measurement matrix
+    H = zeros(1, 14);
+
+    H(1, 1) = OCV_deriv - R0_deriv * u;
+    H(1, 2) = -1;
+    H(1, 3) = -u;
+    H(1, 4) = - u * z;
+    H(1, 5) = - u * z^2;
+    H(1, 6) = - u * z^3;
+
+end
+
+%% Battery Measurement Model
+function y_true = battery_measurement(x, u, a_OCV)
+    z = x(1);
+    V1 = x(2);
+
+    R0_coeff = x(3:6);
+
+    OCV = dot(a_OCV, [1; z; z^2; z^3; z^4; z^5]);
+    R0 = dot(R0_coeff, [1; z; z^2; z^3]);
+
+    y_true = OCV - u * R0 - V1;
+end
+
+%% Battery States Model
+function x_predict = battery_model(x, u, Ts, capacity)
+    z = x(1);
+    V1 = x(2);
+
+    R1_coeff = x(7:10);
+    C1_coeff = x(11:14);
+
+    R1 = dot(R1_coeff, [1; z; z^2; z^3]);
+    C1 = dot(C1_coeff, [1; z; z^2; z^3]);
+    tau = R1 * C1;
+
+    z = z - (u * Ts) / capacity;
+    V1 = V1 * exp(-Ts / tau) + R1 * u * (1 - exp(-Ts / tau));
+
+    x_predict = [z; V1; x(3:end)];
+end
+
+%% Plot Results
+function plot_results(ekf_results)
+
+    datatime = ekf_results.Time;
+    z_true = ekf_results.SOC.true;
+    z_estimated = ekf_results.SOC.estimated;
+    y_true = ekf_results.Voltage.true;
+    y_predict = ekf_results.Voltage.predicted;
+
+    set(0,'DefaultFigureWindowStyle','docked');
+    fontsize = 20;
+    colors = [
+        31, 119, 180;  % Blue
+        255, 127, 14;  % Orange
+        44, 160, 44;   % Green
+        214, 39, 40;   % Red
+        148, 103, 189; % Purple
+    ] / 255;
+
+    figure(1);
+    hold on;
+    grid on;
+    box on;
+    plot(datatime, y_true, 'LineStyle', '--', 'LineWidth', 2, 'Color', colors(1, :));
+    plot(datatime, y_predict, 'LineStyle', '--', 'LineWidth', 2, 'Color', colors(2, :));
+    set(gca, 'fontsize', fontsize);
+    ylabel('Voltage [V]');
+    xlabel('time [s]');
+    legend('True Voltage', 'Predicted Voltage');
+
+    figure(2);
+    hold on;
+    grid on;
+    box on;
+    plot(datatime, z_true, 'LineStyle', '--', 'LineWidth', 2, 'Color', colors(1, :));
+    plot(datatime, z_estimated, 'LineStyle', '--', 'LineWidth', 2, 'Color', colors(2, :));
+    set(gca, 'fontsize', fontsize);
+    ylabel('SOC');
+    xlabel('Time [s]');
+    legend('True SOC', 'Estimated SOC');
+
+end
diff --git a/Lib/simulation_scripts/battery/simulation_realdata_battery_calce.m b/Lib/simulation_scripts/battery/simulation_realdata_battery_calce.m
new file mode 100644
index 0000000..ec1cd58
--- /dev/null
+++ b/Lib/simulation_scripts/battery/simulation_realdata_battery_calce.m
@@ -0,0 +1,172 @@
+%% SIMULATION_REALDATA_BATTERY_SIMULINK
+% file: simulation_general_v3.m
+% author: Federico Oliva
+% date: 27/02/2023
+% description: function to setup and use the MHE observer on battery from
+% real data of CALCE dataset 
+% INPUT: none
+% OUTPUT: params,obs
+function [params,obs] = simulation_realdata_battery_calce
+
+% generate from simulink
+params_sim = params_battery_simulink_calce;
+
+% init observer buffer (see https://doi.org/10.48550/arXiv.2204.09359)
+Nw = 30;
+Nts = 20;
+
+% Tushar setup
+% Nw = 30; % observer window lenth
+% Nts =2; % downsampling factor
+% multirate downsampling
+% temp_Nts = ones(1,Nw) *2;
+% temp_Nts(1:Nw-5)=20; 
+% Nts = temp_Nts;
+
+% noise
+rng default
+
+% set sampling time
+Ts = params_sim.Ts;
+
+% set initial and final time instant
+% remember to set the final time and sampling time accordingly to the data
+% that you measured
+t0 = params_sim.time(1);
+tend = params_sim.time(end);
+% uncomment to test the MHE with a single optimisation step
+% tend = 5*(Nw*Nts-1)*Ts;
+
+%%%%%% general functions
+params_init = @params_battery_calce;
+params_update = @params_update_battery_tushar;
+model = @model_battery_calce;
+% model_reference = @model_battery_calce;
+measure = @measure_battery_tushar;
+% measure_reference = @measure_battery_tushar;
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%%% filters %%%%
+[filter, filterScale, reference] = filter_define(Ts,Nts);
+
+%%%% integration method %%%%
+% ode45-like integration method. For discrete time systems use @odeDD
+ode = @odeDD;
+
+%%%% input law %%%
+input_law = @control_battery;
+
+%%%% params init %%%%
+params = model_init('Ts',Ts,'T0',[t0, tend],'noise',0, 'params_update', params_update, ...
+            'model',model,'measure',measure,'ode',ode, 'odeset', [1e-3 1e-6], ...
+            'input_enable',1,'input_law',input_law,'params_init',params_init,'addons',params_sim);
+             
+%%%% observer init %%%%
+%%%% define arrival cost %%%%%
+terminal_states = params.opt_vars;
+terminal_weights = 1e0*ones(size(terminal_states));
+% SOC%
+% terminal_weights(1) = 5e1;
+% OCV %
+terminal_weights([3 6]) = 1;
+% R0 %
+% terminal_weights([4 8 12]) = 1e-2;
+% OCV %
+% terminal_weights([5 9 13]) = 1e-2;
+% OCV %
+% terminal_weights([6 10 14]) = 1e4;
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+
+% create observer class instance. For more information on the setup
+% options check directly the class constructor in obsopt.m
+obs = obsopt('DataType', 'real', 'optimise', 1, 'MultiStart', params.multistart, 'J_normalise', 1, 'MaxOptTime', Inf, ... 
+          'Nw', Nw, 'Nts', Nts, 'ode', ode, 'PE_maxiter', 0, 'WaitAllBuffer', 0, 'params',params, 'filters', filterScale,'filterTF', filter, ...
+          'Jdot_thresh',0.95,'MaxIter', 1, 'Jterm_store', 1, 'AlwaysOpt', 1 , 'print', 0 , 'SafetyDensity', Inf, 'AdaptiveParams', [10 20 2 20 1 0 0 params.OutDim_compare], ...
+          'AdaptiveSampling',0, 'FlushBuffer', 1, 'opt', @fminsearchcon, 'terminal', 1, 'terminal_states', terminal_states, 'terminal_weights', terminal_weights, 'terminal_normalise', 1, ...
+          'ConPos', [], 'LBcon', [], 'UBcon', [],'NONCOLcon',@nonlcon_fcn,'Bounds', 1,'BoundsPos',[1 4 5],'BoundsValLow',[1e-3 1e-3 1e-3],'BoundsValUp',[1 1e3 1e3]);
+
+
+
+%% %%%% SIMULATION %%%%
+% generate data
+%%%% SWITCH Y WITH THE COLLECTED DATA %%%%%%
+Niter = obs.setup.Niter;
+Ntraj = obs.setup.Ntraj;
+
+% start time counter
+t0 = tic;
+
+% integration loop
+for i = 1:Niter
+    
+    % Display iteration step
+    if ((mod(i,100) == 0) || (i == 1))
+        clc
+        disp('MHE:')
+        disp(['Iteration Number: ', num2str(obs.setup.time(i)),'/',num2str(obs.setup.time(Niter))])
+        disp(['Last J: ',num2str(obs.init.Jstory(end))]);        
+    end
+    
+    % set current iteration in the obsopt class
+    obs.init.ActualTimeIndex = i;
+    obs.init.t = obs.setup.time(obs.init.ActualTimeIndex);    
+    
+    %%%% PROPAGATION %%%%
+    % forward propagation of the previous estimate
+    for traj = 1:Ntraj
+        
+        % set trajectories
+        obs.init.traj = traj;
+        obs_slow.init.traj = traj;
+                 
+        % propagate only if the time gets over the initial time instant
+        if (obs.init.ActualTimeIndex > 1)
+            
+            % define the time span of the integration
+            startpos = obs.init.ActualTimeIndex-1;
+            stoppos = obs.init.ActualTimeIndex;
+            tspan = obs.setup.time(startpos:stoppos); 
+            
+            % fast
+            X = obs.setup.ode(@(t,x)obs.setup.model(t, x, obs.init.params, obs), tspan, obs.init.X_est(traj).val(:,startpos),params.odeset);
+            obs.init.X_est(traj).val(:,startpos:stoppos) = [X.y(:,1),X.y(:,end)];
+                                                      
+        end                
+        
+        %%%% FILTER MEASUREMENT %%%%                 
+        % filter on the measurements    
+        % reference filter (filterScale)
+        y_meas(traj).val = params.y_sim(:,i);            
+    end
+       
+    %%%% OBSERVER %%%%
+    
+    % call the observer                 
+    tfast = tic;
+    obs = obs.observer(obs.init.X_est,y_meas);            
+    obs.init.iter_time(obs.init.ActualTimeIndex) = toc(tfast);                          
+
+end
+% 
+% overall computation time
+obs.init.total_time = toc(t0);
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%% post sim operations %%%
+obs.init.X.val = zeros(params.dim_state,params.Niter);
+obs.init.X.val(1,:) = params_sim.out.simout.ECM_soc.Data';
+obs.init.X.val(3,:) = interp1(params_sim.input_data.SOC,params_sim.input_data.OCV,params_sim.out.simout.ECM_soc.Data);
+obs.init.X.val(4,:) = interp1(params_sim.input_data.SOC,params_sim.input_data.R0,params_sim.out.simout.ECM_soc.Data);
+obs.init.X.val(5,:) = interp1(params_sim.input_data.SOC,params_sim.input_data.R1,params_sim.out.simout.ECM_soc.Data);
+obs.init.X.val(6,:) = interp1(params_sim.input_data.SOC,params_sim.input_data.C1,params_sim.out.simout.ECM_soc.Data);
+
+obs.init.Ytrue_full_story.val = zeros(obs.setup.Nfilt,params.OutDim,params.Niter);
+obs.init.Ytrue_full_story.val(1,1,:) = params_sim.out.simout.ECM_Vb.Data';
+% the whole process could be long, why not going for a nap? No worries, 
+% this "sounds" like a nice way to wake up. (Uncomment)
+% load handel
+% sound(y,Fs)
+obs.init.Nw_Nts=Nts*Nw + 1;
+end
+
diff --git a/Lib/simulation_scripts/battery/simulation_realdata_battery_calce_parallel.m b/Lib/simulation_scripts/battery/simulation_realdata_battery_calce_parallel.m
new file mode 100644
index 0000000..c0c05cc
--- /dev/null
+++ b/Lib/simulation_scripts/battery/simulation_realdata_battery_calce_parallel.m
@@ -0,0 +1,231 @@
+%% SIMULATION_REALDATA_BATTERY_SIMULINK
+% file: simulation_general_v3.m
+% author: Federico Oliva
+% date: 27/02/2023
+% description: function to setup and use the MHE observer on battery from
+% real data of CALCE dataset 
+% INPUT: none
+% OUTPUT: params,obs
+function [params_out,obs] = simulation_realdata_battery_calce_parallel
+
+% generate from simulink
+params_fast = params_battery_simulink_calce;
+params_sim = params_fast;
+clear params_fast
+
+% init observer buffer (see https://doi.org/10.48550/arXiv.2204.09359)
+% Nw = 30;
+
+% noise
+rng default
+
+% set sampling time
+Ts = params_sim.Ts;
+
+% set initial and final time instant
+% remember to set the final time and sampling time accordingly to the data
+% that you measured
+t0 = params_sim.time(1);
+tend = params_sim.time(end);
+% tend = 1000;
+
+%%%%%% general functions
+params_update = @params_update_battery_tushar;
+model = @model_battery_calce;
+% model_reference = @model_battery_calce;
+measure = @measure_battery_tushar;
+% measure_reference = @measure_battery_tushar;
+%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%%% integration method %%%%
+% ode45-like integration method. For discrete time systems use @odeDD
+ode = @odeDD;
+
+%%%% input law %%%
+input_law = @control_battery;
+
+%%% FAST OBSOPT %%%
+
+% params init - fast
+params_fast = model_init('Ts',Ts,'T0',[t0, tend],'noise',0, 'params_update', params_update, ...
+            'model',model,'measure',measure,'ode',ode, 'odeset', [1e-3 1e-6], ...
+            'input_enable',1,'input_law',input_law,'params_init',@params_battery_calce_fast,'addons',params_sim);             
+
+% create observer class instance. For more information on the setup
+% options check directly the class constructor in obsopt.m
+
+% instance for SOC, R0, R1
+Nw_fast = 30;
+% NTs
+Nts_fast = 1;
+
+% filters
+[filter_fast, filterScale_fast, ~] = filter_define(Ts,Nts_fast);
+% terminal states
+terminal_states_fast = params_fast.opt_vars;
+terminal_weights_fast = 1e0*ones(size(terminal_states_fast));
+
+% class instance
+obs_fast = obsopt('DataType', 'real', 'optimise', 1, 'MultiStart', params_fast.multistart, 'J_normalise', 1, 'MaxOptTime', Inf, ... 
+          'Nw', Nw_fast, 'Nts', Nts_fast, 'ode', ode, 'PE_maxiter', 0, 'WaitAllBuffer', 0, 'params',params_fast, 'filters', filterScale_fast,'filterTF', filter_fast, ...
+          'Jdot_thresh',0.95,'MaxIter', 2, 'Jterm_store', 1, 'AlwaysOpt', 1 , 'print', 0 , 'SafetyDensity', Inf, 'AdaptiveParams', [10 20 2 5 1 0 0 params_fast.OutDim_compare], ...
+          'AdaptiveSampling',1, 'FlushBuffer', 1, 'opt', @fminsearchcon, 'terminal', 1, 'terminal_states', terminal_states_fast, 'terminal_weights', terminal_weights_fast, 'terminal_normalise', 1, ...
+          'ConPos', [], 'LBcon', [], 'UBcon', [],'NONCOLcon',@nonlcon_fcn,'Bounds', 1,'BoundsPos',[1 4 5],'BoundsValLow',[1e-3 1e-3 1e-3],'BoundsValUp',[1 1e3 1e3]);
+
+%%% SLOW OBSOPT %%%
+
+% params init - slow
+params_slow = model_init('Ts',Ts,'T0',[t0, tend],'noise',0, 'params_update', params_update, ...
+            'model',model,'measure',measure,'ode',ode, 'odeset', [1e-3 1e-6], ...
+            'input_enable',1,'input_law',input_law,'params_init',@params_battery_calce_slow,'addons',params_sim);             
+
+% create observer class instance. For more information on the setup
+% options check directly the class constructor in obsopt.m
+
+% instance for SOC, R0, R1
+Nw_slow = 30;
+% NTs
+Nts_slow = 100;
+
+% Tushar setup
+% temp_Nts = ones(1,Nw) *2;
+% temp_Nts(1:Nw-5)=20;
+% Nts_slow = temp_Nts;
+
+% filters
+[filter_slow, filterScale_slow, ~] = filter_define(Ts,Nts_slow);
+% terminal states
+terminal_states_slow = params_slow.opt_vars;
+terminal_weights_slow = 1e0*ones(size(terminal_states_slow));
+terminal_weights_slow(1) = 5e1;
+
+% class instance
+obs_slow = obsopt('DataType', 'real', 'optimise', 1, 'MultiStart', params_slow.multistart, 'J_normalise', 1, 'MaxOptTime', Inf, ... 
+          'Nw', Nw_slow, 'Nts', Nts_slow, 'ode', ode, 'PE_maxiter', 0, 'WaitAllBuffer', 0, 'params',params_slow, 'filters', filterScale_slow,'filterTF', filter_slow, ...
+          'Jdot_thresh',0.95,'MaxIter', 2, 'Jterm_store', 1, 'AlwaysOpt', 1 , 'print', 0 , 'SafetyDensity', Inf, 'AdaptiveParams', [10 20 2 50 0.001 0 0 params_slow.OutDim_compare], ...
+          'AdaptiveSampling',1, 'FlushBuffer', 1, 'opt', @fminsearchcon, 'terminal', 1, 'terminal_states', terminal_states_slow, 'terminal_weights', terminal_weights_slow, 'terminal_normalise', 1, ...
+          'ConPos', [], 'LBcon', [], 'UBcon', [],'NONCOLcon',@nonlcon_fcn,'Bounds', 1,'BoundsPos',[1 4 5],'BoundsValLow',[1e-3 1e-3 1e-3],'BoundsValUp',[1 1e3 1e3]);
+
+%% %%%% SIMULATION %%%%
+% generate data
+%%%% SWITCH Y WITH THE COLLECTED DATA %%%%%%
+Niter = obs_fast.setup.Niter;
+Ntraj = obs_fast.setup.Ntraj;
+
+% start time counter
+t0 = tic;
+
+% integration loop
+for i = 1:Niter
+    
+    % Display iteration step
+    if ((mod(i,100) == 0) || (i == 1))
+        clc
+        disp('MHE:')
+        disp(['Iteration Number: ', num2str(obs_fast.setup.time(i)),'/',num2str(obs_fast.setup.time(Niter))])
+        disp(['Last J (fast): ',num2str(obs_fast.init.Jstory(end))]);        
+        disp(['Last J (slow): ',num2str(obs_slow.init.Jstory(end))]);  
+    end
+    
+    % set current iteration in the obsopt class
+    obs_fast.init.ActualTimeIndex = i;
+    obs_fast.init.t = obs_fast.setup.time(obs_fast.init.ActualTimeIndex);    
+
+    obs_slow.init.ActualTimeIndex = i;
+    obs_slow.init.t = obs_slow.setup.time(obs_slow.init.ActualTimeIndex);    
+    
+    %%%% PROPAGATION %%%%
+    % forward propagation of the previous estimate
+    for traj = 1:Ntraj
+        
+        % set trajectories
+        obs_fast.init.traj = traj;
+        obs_fast.init.traj = traj;
+                 
+        % propagate only if the time gets over the initial time instant
+        if (obs_fast.init.ActualTimeIndex > 1)
+            
+            % define the time span of the integration
+            startpos = obs_fast.init.ActualTimeIndex-1;
+            stoppos = obs_fast.init.ActualTimeIndex;
+            tspan = obs_fast.setup.time(startpos:stoppos); 
+            
+            % fast
+            X = obs_fast.setup.ode(@(t,x)obs_fast.setup.model(t, x, obs_fast.init.params, obs_fast), tspan, obs_fast.init.X_est(traj).val(:,startpos),params_fast.odeset);
+            obs_fast.init.X_est(traj).val(:,startpos:stoppos) = [X.y(:,1),X.y(:,end)];
+
+            % slow
+            X = obs_slow.setup.ode(@(t,x)obs_slow.setup.model(t, x, obs_slow.init.params, obs_slow), tspan, obs_slow.init.X_est(traj).val(:,startpos),params_slow.odeset);
+            obs_slow.init.X_est(traj).val(:,startpos:stoppos) = [X.y(:,1),X.y(:,end)];
+                                                      
+        end                
+        
+        %%%% MEASUREMENT %%%%                 
+        y_meas(traj).val = params_fast.y_sim(:,i);            
+    end
+       
+    %%%% OBSERVER %%%%
+    
+    % call the observer                     
+    % fast
+    tfast = tic;
+    obs_fast = obs_fast.observer(obs_fast.init.X_est,y_meas);            
+    obs_fast.init.iter_time(obs_fast.init.ActualTimeIndex) = toc(tfast);                          
+    % slow
+    tslow = tic;
+    obs_slow = obs_slow.observer(obs_slow.init.X_est,y_meas);            
+    obs_slow.init.iter_time(obs_slow.init.ActualTimeIndex) = toc(tslow);
+
+    % slow updates fast
+    if mod(i,Nts_slow) == 0
+        obs_fast.init.X_est(traj).val(params_slow.opt_vars,max(1,i-Nw_slow*Nts_slow):i) = obs_slow.init.X_est(traj).val(params_slow.opt_vars,max(1,i-Nw_slow*Nts_slow):i);
+        obs_slow.init.X_est(traj).val(params_fast.opt_vars,max(1,i-Nw_fast*Nts_fast):i) = obs_fast.init.X_est(traj).val(params_fast.opt_vars,max(1,i-Nw_fast*Nts_fast):i);
+    end
+
+end
+% 
+% overall computation time
+obs_fast.init.total_time = toc(t0);
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%% post sim operations %%%
+obs_fast.init.X.val = zeros(params_fast.dim_state,params_fast.Niter);
+obs_fast.init.X.val(1,:) = params_sim.out.simout.ECM_soc.Data(1:tend+1)';
+obs_fast.init.X.val(3,:) = interp1(params_sim.input_data.SOC,params_sim.input_data.OCV,params_sim.out.simout.ECM_soc.Data(1:tend+1));
+obs_fast.init.X.val(4,:) = interp1(params_sim.input_data.SOC,params_sim.input_data.R0,params_sim.out.simout.ECM_soc.Data(1:tend+1));
+obs_fast.init.X.val(5,:) = interp1(params_sim.input_data.SOC,params_sim.input_data.R1,params_sim.out.simout.ECM_soc.Data(1:tend+1));
+obs_fast.init.X.val(6,:) = interp1(params_sim.input_data.SOC,params_sim.input_data.C1,params_sim.out.simout.ECM_soc.Data(1:tend+1));
+
+obs_fast.init.Ytrue_full_story.val = zeros(obs_fast.setup.Nfilt,params_fast.OutDim,params_fast.Niter);
+obs_fast.init.Ytrue_full_story.val(1,1,:) = params_sim.out.simout.ECM_Vb.Data(1:tend+1)';
+
+
+obs_slow.init.X.val = zeros(params_fast.dim_state,params_fast.Niter);
+obs_slow.init.X.val(1,:) = params_sim.out.simout.ECM_soc.Data(1:tend+1)';
+obs_slow.init.X.val(3,:) = interp1(params_sim.input_data.SOC,params_sim.input_data.OCV,params_sim.out.simout.ECM_soc.Data(1:tend+1));
+obs_slow.init.X.val(4,:) = interp1(params_sim.input_data.SOC,params_sim.input_data.R0,params_sim.out.simout.ECM_soc.Data(1:tend+1));
+obs_slow.init.X.val(5,:) = interp1(params_sim.input_data.SOC,params_sim.input_data.R1,params_sim.out.simout.ECM_soc.Data(1:tend+1));
+obs_slow.init.X.val(6,:) = interp1(params_sim.input_data.SOC,params_sim.input_data.C1,params_sim.out.simout.ECM_soc.Data(1:tend+1));
+
+obs_slow.init.Ytrue_full_story.val = zeros(obs_fast.setup.Nfilt,params_fast.OutDim,params_fast.Niter);
+obs_slow.init.Ytrue_full_story.val(1,1,:) = params_sim.out.simout.ECM_Vb.Data(1:tend+1)';
+
+obs_fast.init.Nw_Nts = Nts_fast*Nw_fast;
+obs_slow.init.Nw_Nts = Nts_slow*Nw_slow;
+
+
+% same ground truth for obs_slow
+obs_slow.init.X = obs_fast.init.X;
+obs_slow.init.Ytrue_full_story = obs_fast.init.Ytrue_full_story;
+
+% assign output
+obs = {obs_fast, obs_slow};
+params_out = {params_fast, params_slow};
+% the whole process could be long, why not going for a nap? No worries, 
+% this "sounds" like a nice way to wake up. (Uncomment)
+% load handel
+% sound(y,Fs)
+
+
+end
+
diff --git a/Lib/simulation_scripts/battery/simulation_realdata_battery_simulink.m b/Lib/simulation_scripts/battery/simulation_realdata_battery_simulink.m
new file mode 100644
index 0000000..cf0794a
--- /dev/null
+++ b/Lib/simulation_scripts/battery/simulation_realdata_battery_simulink.m
@@ -0,0 +1,166 @@
+%% SIMULATION_REALDATA_BATTERY_SIMULINK
+% file: simulation_general_v3.m
+% author: Federico Oliva
+% date: 27/02/2023
+% description: function to setup and use the MHE observer on battery from
+% simulink data (no synthetic model integrated)
+% INPUT: none
+% OUTPUT: params,obs
+function [params,obs] = simulation_realdata_battery_simulink(model_name)
+
+% generate from simulink
+params = params_battery_simulink;
+options = simset('SrcWorkspace','current');
+params.out = sim(model_name,params.time(end),options);
+
+params_sim = params;
+clear params
+
+% init observer buffer (see https://doi.org/10.48550/arXiv.2204.09359)
+Nw = 30;
+Nts = 1;
+
+% noise
+rng default
+
+% set sampling time
+Ts = params_sim.Ts;
+
+% set initial and final time instant
+% remember to set the final time and sampling time accordingly to the data
+% that you measured+
+t0 = params_sim.time(1);
+tend = params_sim.time(end);
+% uncomment to test the MHE with a single optimisation step
+% tend = 10*(Nw*Nts-1)*Ts;
+
+%%%%%% general functions
+params_init = @params_battery_tushar;
+params_update = @params_update_battery_tushar;
+model = @model_battery_tushar;
+model_reference = @model_battery_tushar;
+measure = @measure_battery_tushar;
+measure_reference = @measure_battery_tushar;
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%%% filters %%%%
+[filter, filterScale, reference] = filter_define(Ts,Nts);
+
+%%%% integration method %%%%
+% ode45-like integration method. For discrete time systems use @odeDD
+ode = @odeDD;
+
+%%%% input law %%%
+input_law = @control_battery;
+
+%%%% params init %%%%
+params = model_init('Ts',Ts,'T0',[t0, tend],'noise',0, 'params_update', params_update, ...
+            'model',model,'measure',measure,'ode',ode, 'odeset', [1e-3 1e-6], ...
+            'input_enable',1,'input_law',input_law,'params_init',params_init,'addons',params_sim);
+             
+%%%% observer init %%%%
+%%%% define arrival cost %%%%%
+terminal_states = params.opt_vars;
+terminal_weights = 1e0*ones(size(terminal_states));
+% OCV %
+% terminal_weights([3 7 11]) = 1;
+% R0 %
+% terminal_weights([4 8 12]) = 1e-2;
+% OCV %
+% terminal_weights([5 9 13]) = 1e-2;
+% OCV %
+% terminal_weights([6 10 14]) = 1e4;
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+
+% create observer class instance. For more information on the setup
+% options check directly the class constructor in obsopt.m
+obs = obsopt('DataType', 'simulated', 'optimise', 0, 'MultiStart', params.multistart, 'J_normalise', 1, 'MaxOptTime', Inf, ... 
+          'Nw', Nw, 'Nts', Nts, 'ode', ode, 'PE_maxiter', 0, 'WaitAllBuffer', 0, 'params',params, 'filters', filterScale,'filterTF', filter, ...
+          'Jdot_thresh',0.95,'MaxIter', 1, 'Jterm_store', 1, 'AlwaysOpt', 1 , 'print', 0 , 'SafetyDensity', Inf, 'AdaptiveParams', [4 80 2 1 10 params.OutDim_compare], ...
+          'AdaptiveSampling',0, 'FlushBuffer', 1, 'opt', @fminsearchcon, 'terminal', 1, 'terminal_states', terminal_states, 'terminal_weights', terminal_weights, 'terminal_normalise', 1, ...
+          'ConPos', [], 'LBcon', [], 'UBcon', [],'NONCOLcon',@nonlcon_fcn,'Bounds', 1,'BoundsPos',[1 4 5],'BoundsValLow',[1e-3 1e-3 1e-3],'BoundsValUp',[1 1e3 1e3]);
+
+
+
+%% %%%% SIMULATION %%%%
+% generate data
+%%%% SWITCH Y WITH THE COLLECTED DATA %%%%%%
+Niter = obs.setup.Niter;
+Ntraj = obs.setup.Ntraj;
+
+% start time counter
+t0 = tic;
+
+% integration loop
+for i = 1:Niter
+    
+    % Display iteration step
+    if ((mod(i,1) == 0) || (i == 1))
+        clc
+        disp('MHE:')
+        disp(['Iteration Number: ', num2str(obs.setup.time(i)),'/',num2str(obs.setup.time(Niter))])
+        disp(['Last J: ',num2str(obs.init.Jstory(end))]);        
+    end
+    
+    % set current iteration in the obsopt class
+    obs.init.ActualTimeIndex = i;
+    obs.init.t = obs.setup.time(obs.init.ActualTimeIndex);    
+    
+    %%%% PROPAGATION %%%%
+    % forward propagation of the previous estimate
+    for traj = 1:Ntraj
+        
+        % set trajectories
+        obs.init.traj = traj;
+        obs_slow.init.traj = traj;
+                 
+        % propagate only if the time gets over the initial time instant
+        if (obs.init.ActualTimeIndex > 1)
+            
+            % define the time span of the integration
+            startpos = obs.init.ActualTimeIndex-1;
+            stoppos = obs.init.ActualTimeIndex;
+            tspan = obs.setup.time(startpos:stoppos); 
+            
+            % fast
+            X = obs.setup.ode(@(t,x)obs.setup.model(t, x, obs.init.params, obs), tspan, obs.init.X_est(traj).val(:,startpos),params.odeset);
+            obs.init.X_est(traj).val(:,startpos:stoppos) = [X.y(:,1),X.y(:,end)];
+                                                      
+        end                
+        
+        %%%% FILTER MEASUREMENT %%%%                 
+        % filter on the measurements    
+        % reference filter (filterScale)
+        y_meas(traj).val = params.y_sim(:,i);            
+    end
+       
+    %%%% OBSERVER %%%%
+    
+    % call the observer                 
+    tfast = tic;
+    obs = obs.observer(obs.init.X_est,y_meas);            
+    obs.init.iter_time(obs.init.ActualTimeIndex) = toc(tfast);                          
+
+end
+% 
+% overall computation time
+obs.init.total_time = toc(t0);
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%% post sim operations %%%
+obs.init.X.val = zeros(params.dim_state,params.Niter);
+obs.init.X.val(1,:) = params_sim.out.simout.ECM_soc.Data';
+obs.init.X.val(3,:) = interp1(params_sim.input_data.SOC,params_sim.input_data.OCV,params_sim.out.simout.ECM_soc.Data);
+obs.init.X.val(4,:) = interp1(params_sim.input_data.SOC,params_sim.input_data.R0,params_sim.out.simout.ECM_soc.Data);
+obs.init.X.val(5,:) = interp1(params_sim.input_data.SOC,params_sim.input_data.R1,params_sim.out.simout.ECM_soc.Data);
+obs.init.X.val(6,:) = interp1(params_sim.input_data.SOC,params_sim.input_data.C1,params_sim.out.simout.ECM_soc.Data);
+
+obs.init.Ytrue_full_story.val = zeros(obs.setup.Nfilt,params.OutDim,params.Niter);
+obs.init.Ytrue_full_story.val(1,1,:) = params_sim.out.simout.ECM_Vb.Data';
+% the whole process could be long, why not going for a nap? No worries, 
+% this "sounds" like a nice way to wake up. (Uncomment)
+% load handel
+% sound(y,Fs)
+end
+
diff --git a/Lib/simulation_scripts/rover/simulation_rover.m b/Lib/simulation_scripts/rover/simulation_rover.m
new file mode 100644
index 0000000..aab2938
--- /dev/null
+++ b/Lib/simulation_scripts/rover/simulation_rover.m
@@ -0,0 +1,180 @@
+%% SIMULATION_GENERAL_V3
+% file: simulation_general_v3.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: function to setup and use the MHE observer on general model
+% INPUT: none
+% OUTPUT: params,obs
+function [obs,params] = simulation_rover
+
+%%%% Init Section %%%%
+% uncomment to close previously opened figures
+% close all
+rng('default');
+% rng(42);
+    
+% init observer buffer (see https://doi.org/10.48550/arXiv.2204.09359)
+Nw = 30;
+Nts = 60;
+
+% set sampling time
+Ts = 1e-2;
+
+% set initial and final time instant
+t0 = 0;
+tend = 30;
+% uncomment to test the MHE with a single optimisation step
+% tend = 1*(Nw*Nts-1)*Ts;
+
+%%%% params init function %%%%
+params_init = @params_rover;
+
+%%%% params update function %%%%
+params_update = @params_update_rover;
+
+%%%% model function %%%%
+model = @model_rover;
+
+%%%% model reference function %%%%
+model_reference = @model_rover_reference;
+
+%%%% measure function %%%%
+measure = @measure_rover;
+measure_reference = @measure_rover_reference;
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%%% filters %%%%
+[filter, filterScale] = filter_define(Ts,1);
+
+%%%% integration method %%%%
+ode = @odeEuler;
+
+%%%% input law %%%
+input_law = @control;
+
+%%%% params init %%%%
+params = model_init('Ts',Ts,'T0',[t0, tend],'noise',1, 'params_update', params_update, ...
+            'model',model,'measure',measure,'ode',ode, 'odeset', [1e-3 1e-6], ...
+            'input_enable',1,'input_law',input_law,'params_init',params_init);
+             
+%%%% observer init %%%%
+% defien arrival cost
+terminal_states = params.opt_vars;
+terminal_weights = 1e0*ones(size(terminal_states));
+
+% create observer class instance. For more information on the setup
+% options check directly the class constructor in obsopt.m
+obs = obsopt('DataType', 'simulated', 'optimise', 1, 'MultiStart', params.multistart, 'J_normalise', 1, 'MaxOptTime', Inf, ... 
+          'Nw', Nw, 'Nts', Nts, 'ode', ode, 'PE_maxiter', 0, 'WaitAllBuffer', 2, 'params',params, 'filters', filterScale,'filterTF', filter, ...
+          'model_reference',model_reference, 'measure_reference',measure_reference, ...
+          'Jdot_thresh',0.95,'MaxIter', 5, 'Jterm_store', 1, 'AlwaysOpt', 1 , 'print', 0 , 'SafetyDensity', Inf, 'AdaptiveParams', [4 80 2 1 10 params.pos_acc_out(1:2)], ...
+          'AdaptiveSampling',1, 'FlushBuffer', 1, 'opt', @patternsearch, 'terminal', 0, 'terminal_states', terminal_states, 'terminal_weights', terminal_weights, 'terminal_normalise', 1, ...
+          'ConPos', [], 'LBcon', [], 'UBcon', [],'Bounds', 0,'NONCOLcon',@nonlcon_fcn_rover);
+
+%% %%%% SIMULATION %%%%
+% obs.init.X.val(params.pos_other,1) = 0;
+% obs.init.X_est.val(params.pos_other,1) = 0;
+% start time counter
+t0 = tic;
+
+% integration loop
+for i = 1:obs.setup.Niter
+    
+    % Display iteration step
+    if ((mod(i,10) == 0) || (i == 1))
+        clc
+        disp(['Iteration Number: ', num2str(obs.setup.time(i)),'/',num2str(obs.setup.time(obs.setup.Niter))])
+        disp(['Last J:', num2str(obs.init.Jstory(end))]);
+    end
+    
+    % set current iteration in the obsopt class
+    obs.init.ActualTimeIndex = i;
+    obs.init.t = obs.setup.time(i);
+
+    % define time span (single integration)
+    startpos = max(1,obs.init.ActualTimeIndex-1);
+    stoppos = obs.init.ActualTimeIndex;
+    tspan = obs.setup.time(startpos:stoppos);
+    
+    %%%% PROPAGATION %%%%
+    % forward propagation of the previous estimate    
+        
+    for traj = 1:obs.setup.Ntraj
+        
+        % update traj
+        obs.init.traj = traj;
+             
+        % propagate only if the time gets over the initial time instant
+        if(obs.init.ActualTimeIndex > 1)                
+            
+            % true system - correct initial condition and no noise
+            % considered                 
+            X = obs.setup.ode(@(t,x)obs.setup.model_reference(t, x, obs.setup.params, obs), tspan, obs.init.X(traj).val(:,startpos),params.odeset); 
+            obs.init.X(traj).val(:,startpos:stoppos) = [X.y(:,1),X.y(:,end)];
+    
+            % real system - initial condition perturbed             
+            X = obs.setup.ode(@(t,x)obs.setup.model(t, x, obs.init.params, obs), tspan, obs.init.X_est(traj).val(:,startpos),params.odeset);
+            obs.init.X_est(traj).val(:,startpos:stoppos) = [X.y(:,1),X.y(:,end)]; 
+                      
+        end
+        
+        %%%% REAL MEASUREMENT %%%%
+        % here the noise is noise added aggording to noise_spec
+        [y_meas(traj).val, obs] = obs.setup.measure_reference(obs.init.X(traj).val(:,stoppos),obs.init.params,obs.setup.time(startpos:stoppos),...
+                                                                            obs.init.input_story_ref(traj).val(:,max(1,startpos)),obs);          
+    end
+    
+    %%%% MHE OBSERVER (SAVE MEAS) %%%%
+    t1 = tic;    
+    if ~params.EKF && params.hyb || 0
+        obs = obs.observer(obs.init.X_est,y_meas);
+        obs.init.iter_time(obs.init.ActualTimeIndex) = toc(t1);   
+        if obs.init.break
+            break;
+        end
+    end
+
+    %%% test %%%
+    obs.init.params.UWB_samp_EKF = obs.init.params.UWB_samp;
+    obs.init.params.IMU_samp_EKF = obs.init.params.IMU_samp;    
+
+    %%%% EKF OBSERVER %%%%    
+    t1 = tic;  
+    if (obs.init.ActualTimeIndex > 1) && params.EKF && ~params.hyb
+        for traj=1:params.Ntraj
+            obs.init.traj = traj;
+            obs = EKF_rover(obs,obs.init.X_est(traj).val(:,startpos),y_meas(traj).val);
+        end
+        obs.init.params.UWB_samp_EKF_story = [obs.init.params.UWB_samp_EKF_story obs.init.params.UWB_samp_EKF];
+        obs.init.params.IMU_samp_EKF_story = [obs.init.params.IMU_samp_EKF_story obs.init.params.IMU_samp_EKF];
+    else
+        obs.init.params.UWB_samp_EKF_story = [];
+        obs.init.params.IMU_samp_EKF_story = [];
+    end
+    obs.init.iter_time(obs.init.ActualTimeIndex) = toc(t1);                                                
+    
+    
+
+end
+
+%%%% SNR %%%
+% the SNR is computed on the mesurements https://ieeexplore.ieee.org/document/9805818 
+for i=1:obs.setup.dim_out
+    obs.init.SNR(1).val(i) = 10*log(sum(obs.init.Ytrue_full_story(1).val(1,i,:).^2)/sum(obs.init.noise_story(1).val(i,:).^2));
+end
+
+
+% overall computation time
+obs.init.total_time = toc(t0);
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%% %%%%%%%%%%%%%%%%%%%%%%%%%%%% PLOTS %%%%%%%%%%%%%%%%%%%%%
+% obs self plots
+% obs.plot_section_control(); 
+
+% the whole process could be long, why not going for a nap? No worries, 
+% this "sounds" like a nice way to wake up. (Uncomment)
+% load handel
+% sound(y,Fs)
+end
+
diff --git a/Lib/simulation_scripts/simulation_general.m b/Lib/simulation_scripts/simulation_general.m
index 9f5a497..9cdd407 100644
--- a/Lib/simulation_scripts/simulation_general.m
+++ b/Lib/simulation_scripts/simulation_general.m
@@ -5,24 +5,24 @@
 % description: function to setup and use the MHE observer on general model
 % INPUT: none
 % OUTPUT: params,obs
-function [params,obs] = simulation_general
+function [obs,params] = simulation_general
 
 %%%% Init Section %%%%
 % uncomment to close previously opened figures
 % close all
     
 % init observer buffer (see https://doi.org/10.48550/arXiv.2204.09359)
-Nw = 7;
-Nts = 5;
+Nw = 15;
+Nts = 10;
 
 % set sampling time
-Ts = 5e-2;
+Ts = 1e-1;
 
 % set initial and final time instant
 t0 = 0;
-tend = 10;
+tend = 100;
 % uncomment to test the MHE with a single optimisation step
-% tend = 1*(Nw*Nts+1)*Ts;
+%tend = 1*(Nw*Nts-1)*Ts;
 
 %%%% params init function %%%%
 % function: this function shall be in the following form:
@@ -33,7 +33,7 @@
 % model equations. e.g. for a mechanical system
 % params.M = mass
 % params.b = friction coefficient
-params_init = @params_battery;
+params_init = @params_VolterraLotka;
 
 %%%% params update function %%%%
 % remark: this file is used if the MHE is set to estimate mode parameters
@@ -45,7 +45,7 @@
 % x: state vector
 % OUTPUT: 
 % params_out: updated structure with the new model parameters 
-params_update = @params_update_battery;
+params_update = @params_update_VolterraLotka;
 
 
 %%%% model function %%%%
@@ -58,7 +58,7 @@
 % obs: instance of the obsopt observer class
 % OUTPUT:
 % xdot:output of the state space model
-model = @model_battery;
+model = @model_VolterraLotka;
 
 %%%% model reference function %%%%
 % remark: !DEVEL! this function is used to generate the reference
@@ -74,7 +74,7 @@
 % OUTPUT:
 % xdot:output of the state space model
 % model_reference = @model_reference;
-model_reference = model;
+model_reference = @model_VolterraLotka;
 
 %%%% measure function %%%%
 % function: this file shall be in the following form:   
@@ -86,7 +86,8 @@
 % OUTPUT:
 % y = measure (no noise added). In the following examples it holds
 % y = x(params.observed_state) (see params_init options)
-measure = @measure_battery;
+measure = @measure_general;
+measure_reference = @measure_general;
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 %%%% filters %%%%
@@ -105,7 +106,7 @@
 
 %%%% integration method %%%%
 % ode45-like integration method. For discrete time systems use @odeDD
-ode = @oderk4_fast;
+ode = @odeEuler;
 
 %%%% input law %%%
 % function: defines the input law used. Remember to check the @model
@@ -122,7 +123,8 @@
 % this should be a vector with 2 columns and as many rows as the state
 % dimension. All the noise are considered as Gaussian distributed. The 
 % first column defines the mean while the second column the variance.
-noise_mat = 1*[0,5e-1;0,5e-2];
+noise_mat = 0*ones(5,2);
+% noise_mat(1,2) = 5e-2;
 
 %%%% params init %%%%
 % init the parameters structure through funtion @model_init. 
@@ -130,18 +132,24 @@
 % important is the 'params_init' option, which takes as input the function 
 % handle to the previously defined @params_init. For more information see 
 % directly the model_init.m file.
-params = model_init('Ts',Ts,'T0',[t0, tend],'noise',0,'noise_spec',noise_mat, 'params_update', params_update, ...
-        'model',model,'measure',measure,'ObservedState',[1],'ode',ode, 'odeset', [1e-3 1e-6], ...
-        'input_enable',1,'input_law',input_law,'params_init',params_init);
-
+params = model_init('Ts',Ts,'T0',[t0, tend],'noise',0, 'noise_spec', noise_mat, 'params_update', params_update, ...
+            'model',model,'measure',measure,'ode',ode, 'odeset', [1e-3 1e-6], ...
+            'input_enable',1,'input_law',input_law,'params_init',params_init);
+             
 %%%% observer init %%%%
+% defien arrival cost
+terminal_states = params.opt_vars;
+terminal_weights = 1e-1*ones(size(terminal_states));
+%terminal_weights(3:5) = 10*terminal_weights(3);
+
 % create observer class instance. For more information on the setup
 % options check directly the class constructor in obsopt.m
-obs = obsopt('DataType', 'simulated', 'optimise', 1, ... 
-      'Nw', Nw, 'Nts', Nts, 'ode', ode, 'PE_maxiter', 0, 'control_design', 0 , 'model_reference', model_reference, ...    
-      'params',params, 'filters', filterScale,'filterTF', filter, 'Jdot_thresh',0.9,'MaxIter',60,...
-      'Jterm_store', 1, 'AlwaysOpt', 1 , 'print', 1 , 'SafetyDensity', 3, 'AdaptiveHist', [1e-2, 3e-2, 1e0], ...
-      'AdaptiveSampling',0, 'FlushBuffer', 1, 'opt', @fminunc, 'spring', 0, 'LBcon', [-Inf -Inf 0]);
+obs = obsopt('DataType', 'simulated', 'optimise', 0, 'MultiStart', 0, 'J_normalise', 1, 'MaxOptTime', Inf, ... 
+          'Nw', Nw, 'Nts', Nts, 'ode', ode, 'PE_maxiter', 0, 'WaitAllBuffer', 1, 'params',params, 'filters', filterScale,'filterTF', filter, ...
+          'model_reference',model_reference, 'measure_reference',measure_reference, ...
+          'Jdot_thresh',0.95,'MaxIter', 5, 'Jterm_store', 1, 'AlwaysOpt', 1 , 'print', 0 , 'SafetyDensity', 2, 'AdaptiveFreqMin', [1.5], ...
+          'AdaptiveSampling',0, 'FlushBuffer', 1, 'opt', @fminsearchcon, 'terminal', 0, 'terminal_states', terminal_states, 'terminal_weights', terminal_weights, 'terminal_normalise', 1, ...
+          'ConPos', [], 'LBcon', [], 'UBcon', [],'Bounds', 0);
 
 %% %%%% SIMULATION %%%%
 % remark: the obs.setup.Ntraj variable describes on how many different
@@ -159,6 +167,7 @@
     if ((mod(i,10) == 0) || (i == 1))
         clc
         disp(['Iteration Number: ', num2str(obs.setup.time(i)),'/',num2str(obs.setup.time(obs.setup.Niter))])
+        disp(['Last J:', num2str(obs.init.Jstory(end))]);
     end
     
     % set current iteration in the obsopt class
@@ -168,6 +177,9 @@
     %%%% PROPAGATION %%%%
     % forward propagation of the previous estimate
     for traj = 1:obs.setup.Ntraj
+        
+        % update traj
+        obs.init.traj = traj;
                  
         % propagate only if the time gets over the initial time instant
         if(obs.init.ActualTimeIndex > 1)
@@ -175,21 +187,23 @@
             % define the time span of the integration
             startpos = obs.init.ActualTimeIndex-1;
             stoppos = obs.init.ActualTimeIndex;
-            tspan = obs.setup.time(startpos:stoppos);            
-
+            tspan = obs.setup.time(startpos:stoppos);   
+            
             % true system - correct initial condition and no noise
-            % considered
-            X = obs.setup.ode(@(t,x)obs.setup.model_reference(t, x, params, obs), tspan, obs.init.X(traj).val(:,startpos),params.odeset); 
-            obs.init.X(traj).val(:,startpos:stoppos) = X.y;
+            % considered                 
+            X = obs.setup.ode(@(t,x)obs.setup.model_reference(t, x, obs.setup.params, obs), tspan, obs.init.X(traj).val(:,startpos),params.odeset); 
+            obs.init.X(traj).val(:,startpos:stoppos) = [X.y(:,1),X.y(:,end)];
 
-            % real system - initial condition perturbed 
+            % real system - initial condition perturbed             
             X = obs.setup.ode(@(t,x)obs.setup.model(t, x, obs.init.params, obs), tspan, obs.init.X_est(traj).val(:,startpos),params.odeset);
-            obs.init.X_est(traj).val(:,startpos:stoppos) = X.y;      
+            obs.init.X_est(traj).val(:,startpos:stoppos) = [X.y(:,1),X.y(:,end)]; 
+                      
         end
         
         %%%% REAL MEASUREMENT %%%%
         % here the noise is noise added aggording to noise_spec
-        obs.init.Ytrue_full_story(traj).val(1,:,obs.init.ActualTimeIndex) = obs.setup.measure(obs.init.X(traj).val(:,obs.init.ActualTimeIndex),obs.init.params,obs.setup.time(obs.init.ActualTimeIndex));
+        obs.init.Ytrue_full_story(traj).val(1,:,obs.init.ActualTimeIndex) = obs.setup.measure_reference(obs.init.X(traj).val(:,obs.init.ActualTimeIndex),obs.init.params,obs.setup.time(obs.init.ActualTimeIndex),...
+                                                                            obs.init.input_story_ref(traj).val(:,max(1,obs.init.ActualTimeIndex-1)));
         obs.init.noise_story(traj).val(:,obs.init.ActualTimeIndex) = obs.setup.noise*(obs.setup.noise_mu(obs.setup.observed_state)  + obs.setup.noise_std(obs.setup.observed_state).*randn(obs.setup.dim_out,1));
         y_meas(traj).val =  reshape(obs.init.Ytrue_full_story(traj).val(1,:,obs.init.ActualTimeIndex),obs.setup.dim_out,1) + obs.init.noise_story(traj).val(:,obs.init.ActualTimeIndex);
     end
@@ -200,10 +214,7 @@
     
     % call the observer
     obs = obs.observer(obs.init.X_est,y_meas);
-    
-    % update the model parameters
-    params = obs.init.params;
-    
+   
     % stop time counter for the observer. Each optimisation process is
     % timed.
     obs.init.iter_time(obs.init.ActualTimeIndex) = toc(t1);
@@ -211,7 +222,7 @@
 end
 
 %%%% SNR %%%
-% the SNR is computed on the mesurements
+% the SNR is computed on the mesurements https://ieeexplore.ieee.org/document/9805818 
 for traj = 1:obs.setup.Ntraj
     for i=1:obs.setup.dim_out
         obs.init.SNR(traj).val(i) = 10*log(sum(obs.init.Ytrue_full_story(traj).val(1,i,:).^2)/sum(obs.init.noise_story(traj).val(i,:).^2));
@@ -224,7 +235,7 @@
 
 %% %%%%%%%%%%%%%%%%%%%%%%%%%%%% PLOTS %%%%%%%%%%%%%%%%%%%%%
 % obs self plots
-obs.plot_section_control(); 
+% obs.plot_section_control(); 
 
 % the whole process could be long, why not going for a nap? No worries, 
 % this "sounds" like a nice way to wake up. (Uncomment)
diff --git a/Lib/various/Annihilator/src/annihilator.m b/Lib/various/Annihilator/src/annihilator.m
new file mode 100644
index 0000000..961615e
--- /dev/null
+++ b/Lib/various/Annihilator/src/annihilator.m
@@ -0,0 +1,58 @@
+function [num_An, N, N_An] = annihilator(sys, eta)
+%ANNIHILATOR Dynamic annihilator.
+%
+%   [NUM_AN] = ANNIHILATOR(SYS, ETA) computes the dynamic annihilator of the
+%   SISO or MIMO linear system SYS:
+%
+%     NUM(s) * NUM_An(s) = 0, for each s.
+%
+%   [NUM_AN, N, N_AN] = ANNIHILATOR(SYS, ETA) also returns the cell array 
+%   with the coefficients of the polynomials to the numerator of the
+%   related systems:
+%
+%     N(s) = N_0 + N_1 * s + ... + N_n * s^n
+%     N_An(s) = N_An_0 + N_An_1 * s + ... + N_An_eta * s^eta
+%
+% Alessandro Tenaglia <alessandro.tenaglia42@gmail.com>
+% October 2, 2022
+
+% Get the system dimensions
+n = size(sys.A, 1);
+m = size(sys.B, 2);
+p = size(sys.C, 1);
+% Compute a left comprime factorization
+[N, ~, ~] = lcf(sys);
+% Compose the matrix that defines the relationships that 
+% the annihilator must satisfy
+M = zeros(p * (eta+n), m * eta);
+for k = 1 : n
+  for h = 1 : eta
+    i1 = (k-1) * p + (h-1) * p + 1;
+    i2 = (k-1) * p + h * p;
+    j1 = (h-1) * m + 1;
+    j2 = h * m;
+    M(i1:i2, j1:j2) = N{k};
+  end
+end
+% Compute the null of the matrix
+Mperp = null(M);
+q = size(Mperp, 2);
+if (q == 0)
+  error('No annhiliator')
+end
+% Compose the matrices of coefficients of the annihilator numerator
+N_An = cellmat(1, eta+1, m, q);
+for k = 1 : eta
+  j1 = (k-1) * m + 1;
+  j2 = k * m;
+  N_An{k} = Mperp(j1:j2, :);
+end
+% Compose the annihilator numerator
+num_An = cellmat(m, q, 1, eta+1);
+for j = 1 : m
+  for i = 1 : q
+    for k = 1 : eta+1
+      num_An{j, i}(eta-k+2) = N_An{k}(j, i);
+    end
+  end
+end
diff --git a/Lib/various/Annihilator/src/lcf.m b/Lib/various/Annihilator/src/lcf.m
new file mode 100644
index 0000000..e1f35c6
--- /dev/null
+++ b/Lib/various/Annihilator/src/lcf.m
@@ -0,0 +1,69 @@
+function [N, d, W] = lcf(sys)
+%LCF Left coprime factorization.
+%
+%   LCF computes a left coprime factorization of the
+%   SISO or MIMO linear system SYS:
+%
+%     W(s) = d^(-1)(s) * N(s),
+%
+%   where d(s) and N(s) are coprime polynomial matrices.
+%
+%   [N, d] = LCF(SYS) returns the cell array with the coefficients 
+%   of the polynomials to the numerator and the vector with the 
+%   coefficients of the polynomial to the denominator:
+%
+%     N(s) = N_0 + N_1 * s + ... + N_n * s^n.
+%     d(s) = d_0 + d_1 * s + ... + d_n * s^n.
+%
+%   [N, d, W] = LCF(SYS) also returns the corresponding transfer function.
+%
+% Alessandro Tenaglia <alessandro.tenaglia42@gmail.com>
+% October 2, 2022
+
+% Store the system matrices locally
+A = sys.A;
+B = sys.B;
+C = sys.C;
+D = sys.D;
+% Get the system dimensions
+n = size(A, 1);
+m = size(B, 2);
+p = size(C, 1);
+% Initializes the matrices of coefficients of adj(sI-A)
+%   ->  adj(sI-A) = E_{n-1} * s^{n-1} + ... + E_1 * s + E_0 
+E = cellmat(1, n+1, n, n);
+E{n+1} = zeros(n, n);
+% Initializes the vector of coefficients of det(sI-A)
+%   ->  det(sI-A) = s^n + d_{n-1} * s^{n-1} + ... + d_1 * s + d_0 
+d = zeros(1, n+1);
+d(n+1) = 1;
+% Iteratively calculate the coefficients using
+% the Souriau-Leverrier-Faddeev algorithm
+for k = 1 : n
+  E{n-k+1} = d(n-k+2) * eye(n) + A * E{n-k+2};
+  d(n-k+1) = -1/k * trace(A * E{n-k+1});
+end
+% Check the correctness of the algorithm
+err = max(abs(d(1) * eye(n) + A * E{1}), [], 'all');
+if err > 1e-3
+  warning('Algorithm error: %s', char(err))
+end
+% Compute the matrices of coefficients of N(s)
+%   ->  N(s) = N_0 + N_1 * s + ... + N_n * s^n
+N = cellmat(1, n+1, p, m);
+for k = 1 : n+1
+  N{k} = C * E{k} * B + d(k) * D;
+end
+% Compose the transfer function numerator
+num = cellmat(p, m, 1, n+1);
+for i = 1 : p
+  for j = 1 : m
+    for k = 1 : n+1
+      num{i, j}(n-k+2) = N{k}(i, j);
+    end
+  end
+end
+% Compose the transfer function denominator
+den = fliplr(d);
+% Compute the transfer function
+W = tf(num, den);
diff --git a/Lib/various/Annihilator/src/test_allocation.m b/Lib/various/Annihilator/src/test_allocation.m
new file mode 100644
index 0000000..4eef13f
--- /dev/null
+++ b/Lib/various/Annihilator/src/test_allocation.m
@@ -0,0 +1,217 @@
+clearvars
+close all
+clc
+
+%% Plant
+A = [-0.157, -0.094;
+     -0.416, -0.45];
+B = [0.87, 0.253, 0.743;
+     0.39, 0.354, 0.65];
+C = [0.0, 1.0];
+D = [0.0, 0.0, 0.0];
+
+sys_P = ss(A,B,C,D);
+
+x0 = zeros(2, 1);
+
+n = size(A, 1);
+m = size(B, 2);
+p = size(C, 1);
+
+%% Controller
+Ac = [-1.57, 0.5767, 0.822, -0.65;
+     -0.9, -0.501, -0.94, 0.802;
+     0.0, 1.0, -1.61, 1.614;
+     0.0, 0.0, 0.0, 0.0];
+Bc = [0.0;
+      0.0;
+      0.0;
+      1.0];
+Cc = [1.81, -1.2, -0.46, 0.0;
+      -0.62, 1.47, 0.89, 0.0
+      0.0, 0.0, 0.0, 0.0];
+Dc = [0.0;
+      0.0;
+      0.0];
+
+xc0 = zeros(4, 1);
+
+%% Allocator with static annihilator
+% Static annihilator
+Pstar = dcgain(sys_P);
+Pperp = null(Pstar);
+
+q = size(Pperp, 2);
+
+% Optimizer
+gamma_all = 0.1;
+R_all = eye(m);
+
+Ag_all = zeros(q);
+Bg_all = -gamma_all * Pperp' * R_all;
+
+xg0_all = zeros(q, 1);
+
+%% Allocator with dynamic annihilator
+% Dynamic annihilator
+[sys_An, ~, ~] = annihilator(sys_P);
+Aan = sys_An.A;
+Ban = sys_An.B;
+Can = sys_An.C;
+Dan = sys_An.D;
+
+xan0_ann = zeros(size(Aan, 1), 1);
+
+q = size(Ban, 2);
+
+% Optimizer
+gamma_ann = 1e8;
+R_ann = eye(m);
+Anstar = dcgain(sys_An);
+
+Ag_ann = zeros(q);
+Bg_ann = -gamma_ann * Anstar' * R_ann;
+xg0_ann = zeros(q, 1);
+
+%%
+t0 = 0; dt = 1e-3; T = 50;
+t = t0:dt:T;
+r = ones(1, numel(t));
+
+% Error
+e = zeros(1, numel(t));
+% Controller
+xc = xc0;
+% Allocator
+J = zeros(1, numel(t));
+% Input
+u = zeros(m, numel(t));
+% Plant
+x = x0;
+y = zeros(1, numel(t));
+% Iterate on time
+for it = 1 : numel(t)
+  % Error
+  e(it) = r(it) - y(max(it-1, 1));
+  % Controller
+  yc = Cc * xc + Dc * e(it);
+  xc = xc + dt * (Ac * xc + Bc * e(it));
+  % Input
+  u(:, it) = yc;
+  J(it) = 1/2 * u(:, it)' * R_all * u(:, it);
+  % Plant
+  y(it) = C * x + D * u(:, it);
+  x = x + dt * (A * x + B * u(:, it));
+end
+
+% Error
+e_all = zeros(1, numel(t));
+% Controller
+xc = xc0;
+% Allocator
+xg_all = xg0_all;
+J_all = zeros(1, numel(t));
+% Input
+u_all = zeros(m, numel(t));
+% Plant
+x = x0;
+y_all = zeros(1, numel(t));
+% Iterate on time
+for it = 1 : numel(t)
+  % Error
+  e_all(it) = r(it) - y_all(max(it-1, 1));
+  % Controller
+  yc = Cc * xc + Dc * e_all(it);
+  xc = xc + dt * (Ac * xc + Bc * e_all(it));
+  % Allocator
+  ya_all = Pperp * xg_all;
+  xg_all = xg_all + dt * (Ag_all * xg_all + Bg_all * (yc + ya_all));
+  % Input
+  u_all(:, it) = yc + ya_all;
+  J_all(it) = 1/2 * u_all(:, it)' * R_all * u_all(:, it);
+  % Plant
+  y_all(it) = C * x + D * u_all(:, it);
+  x = x + dt * (A * x + B * u_all(:, it));
+end
+
+% Error
+e_ann = zeros(1, numel(t));
+% Controller
+xc = xc0;
+% Allocator
+xg_ann = xg0_ann;
+J_ann = zeros(1, numel(t));
+xa_ann = xan0_ann;
+% Input
+u_ann = zeros(m, numel(t));
+% Plant
+x = x0;
+y_ann = zeros(1, numel(t));
+% Iterate on time
+for it = 1 : numel(t)
+  % Error
+  e_ann(it) = r(it) - y_ann(max(it-1, 1));
+  % Controller
+  yc = Cc * xc + Dc * e_ann(it);
+  xc = xc + dt * (Ac * xc + Bc * e_ann(it));
+  % Allocator
+  ya_ann = Can * xa_ann;
+  xg_ann = xg_ann + dt * (Ag_ann * xg_ann + Bg_ann * (yc + ya_ann));
+  xa_ann = xa_ann + dt * (Aan * xa_ann + Ban * xg_ann);
+  % Input
+  u_ann(:, it) = yc + ya_ann;
+  J_ann(it) = 1/2 * u_ann(:, it)' * R_ann * u_ann(:, it);
+  % Plant
+  y_ann(it) = C * x + D * u_ann(:, it);
+  x = x + dt * (A * x + B * u_ann(:, it));
+end
+
+figure()
+subplot(3, 2, 1)
+hold on; grid on;
+h1 = plot(t, r, '-k');
+h2 = plot(t, y, '-r');
+h3 = plot(t, y_all, '-g');
+h4 = plot(t, y_ann, '-b');
+xlim([t0 T])
+legend([h1, h2, h3, h4], 'ref', 'no all', 'sta', 'dyn')
+title('Output')
+subplot(3, 2, 3)
+hold on; grid on;
+h1 = plot(t, y - y_all, '-g');
+h2 = plot(t, y - y_ann, '-b');
+xlim([t0 T])
+legend([h1, h2], 'sta', 'dyn')
+title('Output variation')
+subplot(3, 2, 5)
+hold on; grid on;
+h1 = plot(t, J, '-r');
+h2 = plot(t, J_all, '-g');
+h3 = plot(t, J_ann, '-b');
+xlim([t0 T])
+legend([h1, h2, h3], 'no all', 'sta', 'dyn')
+title('Cost function')
+subplot(3, 2, 2)
+hold on; grid on;
+h1 = plot(t, u(1, :), '-r');
+h2 = plot(t, u_all(1, :), '-g');
+h3 = plot(t, u_ann(1, :), '-b');
+xlim([t0 T])
+legend([h1, h2, h3], 'no all', 'sta', 'dyn')
+title('Input 1')
+subplot(3, 2, 4)
+hold on; grid on;
+h1 = plot(t, u(2, :), '-r');
+h2 = plot(t, u_all(2, :), '-g');
+h3 = plot(t, u_ann(2, :), '-b');
+xlim([t0 T])
+legend([h1, h2, h3], 'no all', 'sta', 'dyn')
+title('Input 2')
+subplot(3, 2, 6)
+hold on; grid on;
+h1 = plot(t, u(3, :), '-r');
+h2 = plot(t, u_all(3, :), '-g');
+h3 = plot(t, u_ann(3, :), '-b');
+xlim([t0 T])
+legend([h1, h2, h3], 'no all', 'sta', 'dyn')
+title('Input 3')
diff --git a/Lib/various/Annihilator/src/test_annihilator.m b/Lib/various/Annihilator/src/test_annihilator.m
new file mode 100644
index 0000000..58aa618
--- /dev/null
+++ b/Lib/various/Annihilator/src/test_annihilator.m
@@ -0,0 +1,37 @@
+clearvars
+close all
+clc
+
+%% Annihilator Test
+N = 10;
+err = zeros(N, 1);
+fprintf('Annihilator Test\n')
+
+% Iterate N times
+for i=1:N
+    % Generate randomic dimensions
+    n = max(randi(11), 2);
+    m = max(randi(n-1), 2);
+    p = randi(m-1);
+    % Generate a stable system with randomic dimensions
+    sys_P = rss(n, p, m);
+
+    % Compute a dynamic annihilator for the system
+    try
+        [sys_An, ~, ~, ~] = annihilator(sys_P);
+    catch
+       ARARMAX = 1; 
+    end
+
+    % Check the behavior of the series between the system and 
+    % the annihilator
+    try
+        sys = series(sys_An, sys_P);
+    catch
+       ARARMAX = 1; 
+    end
+    y = step(sys);
+    err(i) = max(y, [], 'all');
+end
+
+fprintf('Max error %f\n', max(err, [], 'all'))
diff --git a/Lib/various/Annihilator/testF.m b/Lib/various/Annihilator/testF.m
new file mode 100644
index 0000000..9e6cfa1
--- /dev/null
+++ b/Lib/various/Annihilator/testF.m
@@ -0,0 +1,38 @@
+%%
+function [F,Fall_init,Fall_opt] = testF(params_init,params_opt)
+
+    % Op init
+    Anstar_init = dcgain(params_init.sys_An);
+    A_op_init = -params_init.gamma*Anstar_init'*params_init.R*Anstar_init;
+    B_op_init = -params_init.gamma*Anstar_init'*params_init.R;
+    C_op_init = eye(size(A_op_init));
+    D_op_init = zeros(size(C_op_init,1),size(B_op_init,2));
+    Sigma_op_init = ss(A_op_init,B_op_init,C_op_init,D_op_init);
+    
+    % Sigma_A
+    Sigma_A_init = Sigma_op_init*params_init.sys_An;
+    
+    % Op optimised
+    Anstar_opt = dcgain(params_opt.sys_An);
+    A_op_opt = -params_opt.gamma*Anstar_opt'*params_opt.R*Anstar_opt;
+    B_op_opt = -params_opt.gamma*Anstar_opt'*params_opt.R;
+    C_op_opt = eye(size(A_op_opt));
+    D_op_opt = zeros(size(C_op_opt,1),size(B_op_opt,2));
+    Sigma_op_opt = ss(A_op_opt,B_op_opt,C_op_opt,D_op_opt);
+    
+    % Sigma_A
+    Sigma_A_opt = Sigma_op_opt*params_opt.sys_An;
+    
+    % F
+    L = params_init.sys_C*params_init.sys_P;
+    F = L/(1+L);
+    
+    % Fall init
+    Lall = params_init.sys_C*(1+Sigma_A_init)*params_init.sys_P;
+    Fall_init = Lall/(1+Lall);
+    
+    % Fall init
+    Lall = params_opt.sys_C*(1+Sigma_A_opt)*params_opt.sys_P;
+    Fall_opt = Lall/(1+Lall);
+    
+end
diff --git a/Lib/various/Derivative/PseudoDer.m b/Lib/various/Derivative/PseudoDer.m
new file mode 100644
index 0000000..bf67293
--- /dev/null
+++ b/Lib/various/Derivative/PseudoDer.m
@@ -0,0 +1,61 @@
+% ITERATIVE_PSEUDO_DERIVATIVE  Compute time derivative over a buffer 
+% dy = iterative_pseudo_derivative(Ts, y, wlen, bufsize, median, reset) 
+% Ts: Sampling time 
+% y: New data sample 
+% wlen: Length of the two subwindows where the median/mean is evaluated 
+% bufsize: Samples buffer length 
+% median: Flag to select median computation over buffer or sample average 
+% reset: Flag to discard the data buffer 
+
+% (------bufsize------) 
+% (-wlen-)-----(-wlen-) 
+% [------]-----[------] 
+
+function [dy, der_buffer, counter]  = PseudoDer(Ts,y,wlen,bufsize,signalsize,median,reset,obs,der_buffer, counter)   
+  
+     % Reset buffer if required 
+     if(isempty(der_buffer) || (reset)) 
+         for traj = 1:obs.init.params.Ntraj
+            der_buffer(traj).val = zeros(signalsize,bufsize); 
+         end
+        
+        counter = 0; 
+     end 
+  
+     % Update buffer by shifting old samples and store new one 
+     for k = 1:(bufsize-1) 
+        der_buffer(obs.init.traj).val(:,k) = der_buffer(obs.init.traj).val(:,k+1); 
+     end 
+
+     % update 
+     counter = counter+1; 
+     der_buffer(obs.init.traj).val(:,bufsize) = y; 
+  
+     % Compute derivative over buffer (if enough data has been collected) 
+     if (counter >= bufsize) 
+        temp1 = 0; 
+        temp2 = 0; 
+      
+        if (median) 
+            % Current and future values are computed using medians 
+            temp1 = median(der_buffer(obs.init.traj).val(:,1:wlen),2); 
+            temp2 = median(der_buffer(obs.init.traj).val(:,(bufsize-wlen+1):bufsize),2); 
+        else 
+
+            % Current and future values are averaged over the subwindows 
+            for k=1:wlen 
+                temp1 = temp1+der_buffer(obs.init.traj).val(:,k); 
+            end 
+            temp1 = temp1/wlen; 
+      
+            for k=(bufsize-wlen+1):bufsize 
+                temp2 = temp2 + der_buffer(obs.init.traj).val(:,k); 
+            end 
+            temp2 = temp2/wlen; 
+        end 
+      
+        dy = (temp2-temp1)/(Ts*(bufsize-wlen)); 
+     else 
+        dy = zeros(signalsize,1); 
+     end 
+ end
\ No newline at end of file
diff --git a/Lib/various/FMINSEARCHBND/FMINSEARCHBND/demo/fminsearchbnd_demo.m b/Lib/various/FMINSEARCHBND/FMINSEARCHBND/demo/fminsearchbnd_demo.m
new file mode 100644
index 0000000..53623ab
--- /dev/null
+++ b/Lib/various/FMINSEARCHBND/FMINSEARCHBND/demo/fminsearchbnd_demo.m
@@ -0,0 +1,47 @@
+%% Optimization of a simple (Rosenbrock) function, with no constraints
+rosen = @(x) (1-x(1)).^2 + 105*(x(2)-x(1).^2).^2;
+
+% With no constraints, operation simply passes through
+% directly to fminsearch. The solution should be [1 1]
+xsol = fminsearchbnd(rosen,[3 3])
+
+%% Only lower bound constraints
+xsol = fminsearchbnd(rosen,[3 3],[2 2])
+
+%% Only upper bound constraints
+xsol = fminsearchbnd(rosen,[-5 -5],[],[0 0])
+
+%% Dual constraints
+xsol = fminsearchbnd(rosen,[2.5 2.5],[2 2],[3 3])
+
+%% Mixed constraints
+xsol = fminsearchbnd(rosen,[0 0],[2 -inf],[inf 3])
+
+%% Provide your own fminsearch options
+opts = optimset('fminsearch');
+opts.Display = 'iter';
+opts.TolX = 1.e-12;
+opts.MaxFunEvals = 100;
+
+n = [10,5];
+H = randn(n);
+H=H'*H;
+Quadraticfun = @(x) x*H*x';
+
+% Global minimizer is at [0 0 0 0 0].
+% Set all lower bound constraints, all of which will
+% be active in this test.
+LB = [.5 .5 .5 .5 .5];
+xsol = fminsearchbnd(Quadraticfun,[1 2 3 4 5],LB,[],opts)
+
+%% Exactly fix one variable, constrain some others, and set a tolerance
+opts = optimset('fminsearch');
+opts.TolFun = 1.e-12;
+
+LB = [-inf 2 1 -10];
+UB = [ inf  inf 1  inf];
+xsol = fminsearchbnd(@(x) norm(x),[1 3 1 1],LB,UB,opts)
+
+%% All the standard outputs from fminsearch are still returned
+[xsol,fval,exitflag,output] = fminsearchbnd(@(x) norm(x),[1 3 1 1],LB,UB)
+
diff --git a/Lib/various/FMINSEARCHBND/FMINSEARCHBND/demo/fminsearchcon_demo.m b/Lib/various/FMINSEARCHBND/FMINSEARCHBND/demo/fminsearchcon_demo.m
new file mode 100644
index 0000000..f8cda09
--- /dev/null
+++ b/Lib/various/FMINSEARCHBND/FMINSEARCHBND/demo/fminsearchcon_demo.m
@@ -0,0 +1,61 @@
+%% Optimization of a simple (Rosenbrock) function
+rosen = @(x) (1-x(1)).^2 + 105*(x(2)-x(1).^2).^2;
+
+% With no constraints, operation simply passes through
+% directly to fminsearch. The solution should be [1 1]
+xsol = fminsearchcon(rosen,[3 3])
+
+%% Only lower bound constraints
+xsol = fminsearchcon(rosen,[3 3],[2 2])
+
+%% Only upper bound constraints
+xsol = fminsearchcon(rosen,[-5 -5],[],[0 0])
+
+%% Dual constraints
+xsol = fminsearchcon(rosen,[2.5 2.5],[2 2],[3 3])
+
+%% Mixed constraints
+xsol = fminsearchcon(rosen,[0 0],[2 -inf],[inf 3])
+
+%% Fix a variable as constant, x(2) == 3
+fminsearchcon(rosen,[3 3],[-inf 3],[inf,3])
+
+%% Linear inequality, x(1) + x(2) <= 1
+fminsearchcon(rosen,[0 0],[],[],[1 1],1)
+
+%% Nonlinear inequality, norm(x) <= 1
+fminsearchcon(rosen,[0 0],[],[],[],[],@(x) norm(x) - 1)
+
+%% Minimize a linear objective, subject to a nonlinear constraints.
+fun = @(x) x*[-2;1];
+nonlcon = @(x) [norm(x) - 1;sin(sum(x))];
+fminsearchcon(fun,[0 0],[],[],[],[],nonlcon)
+
+%% Provide your own fminsearch options
+opts = optimset('fminsearch');
+opts.Display = 'iter';
+opts.TolX = 1.e-12;
+opts.MaxFunEvals = 100;
+
+n = [10,5];
+H = randn(n);
+H=H'*H;
+Quadraticfun = @(x) x*H*x';
+
+% Global minimizer is at [0 0 0 0 0].
+% Set all lower bound constraints, all of which will
+% be active in this test.
+LB = [.5 .5 .5 .5 .5];
+xsol = fminsearchcon(Quadraticfun,[1 2 3 4 5],LB,[],[],[],[],opts)
+
+%% Exactly fix one variable, constrain some others, and set a tolerance
+opts = optimset('fminsearch');
+opts.TolFun = 1.e-12;
+
+LB = [-inf 2 1 -10];
+UB = [ inf  inf 1  inf];
+xsol = fminsearchcon(@(x) norm(x),[1 3 1 1],LB,UB,[],[],[],opts)
+
+%% All the standard outputs from fminsearch are still returned
+[xsol,fval,exitflag,output] = fminsearchcon(@(x) norm(x),[1 3 1 1],LB,UB)
+
diff --git a/Lib/various/FMINSEARCHBND/FMINSEARCHBND/demo/html/fminsearchbnd_demo.html b/Lib/various/FMINSEARCHBND/FMINSEARCHBND/demo/html/fminsearchbnd_demo.html
new file mode 100644
index 0000000..86fb926
--- /dev/null
+++ b/Lib/various/FMINSEARCHBND/FMINSEARCHBND/demo/html/fminsearchbnd_demo.html
@@ -0,0 +1,299 @@
+<html xmlns:mwsh="http://www.mathworks.com/namespace/mcode/v1/syntaxhighlight.dtd">
+   <head>
+      <meta http-equiv="Content-Type" content="text/html; charset=utf-8">
+   
+      <!--
+This HTML is auto-generated from an M-file.
+To make changes, update the M-file and republish this document.
+      -->
+      <title>fminsearchbnd_demo</title>
+      <meta name="generator" content="MATLAB 7.0.1">
+      <meta name="date" content="2006-07-24">
+      <meta name="m-file" content="fminsearchbnd_demo"><style>
+body {
+  background-color: white;
+  margin:10px;
+}
+h1 {
+  color: #990000; 
+  font-size: x-large;
+}
+h2 {
+  color: #990000;
+  font-size: medium;
+}
+p.footer {
+  text-align: right;
+  font-size: xx-small;
+  font-weight: lighter;
+  font-style: italic;
+  color: gray;
+}
+
+pre.codeinput {
+  margin-left: 30px;
+}
+
+span.keyword {color: #0000FF}
+span.comment {color: #228B22}
+span.string {color: #A020F0}
+span.untermstring {color: #B20000}
+span.syscmd {color: #B28C00}
+
+pre.showbuttons {
+  margin-left: 30px;
+  border: solid black 2px;
+  padding: 4px;
+  background: #EBEFF3;
+}
+
+pre.codeoutput {
+  color: gray;
+  font-style: italic;
+}
+pre.error {
+  color: red;
+}
+
+/* Make the text shrink to fit narrow windows, but not stretch too far in 
+wide windows.  On Gecko-based browsers, the shrink-to-fit doesn't work. */ 
+p,h1,h2,div {
+  /* for MATLAB's browser */
+  width: 600px;
+  /* for Mozilla, but the "width" tag overrides it anyway */
+  max-width: 600px;
+  /* for IE */
+  width:expression(document.body.clientWidth > 620 ? "600px": "auto" );
+}
+
+    </style></head>
+   <body>
+      <h2>Contents</h2>
+      <div>
+         <ul>
+            <li><a href="#1">Optimization of a simple (Rosenbrock) function, with no constraints</a></li>
+            <li><a href="#2">Only lower bound constraints</a></li>
+            <li><a href="#3">Only upper bound constraints</a></li>
+            <li><a href="#4">Dual constraints</a></li>
+            <li><a href="#5">Mixed constraints</a></li>
+            <li><a href="#6">Provide your own fminsearch options</a></li>
+            <li><a href="#7">Exactly fix one variable, constrain some others, and set a tolerance</a></li>
+            <li><a href="#8">All the standard outputs from fminsearch are still returned</a></li>
+         </ul>
+      </div>
+      <h2>Optimization of a simple (Rosenbrock) function, with no constraints<a name="1"></a></h2><pre class="codeinput">rosen = @(x) (1-x(1)).^2 + 105*(x(2)-x(1).^2).^2;
+
+<span class="comment">% With no constraints, operation simply passes through</span>
+<span class="comment">% directly to fminsearch. The solution should be [1 1]</span>
+xsol = fminsearchbnd(rosen,[3 3])
+</pre><pre class="codeoutput">
+xsol =
+
+      0.99998      0.99995
+
+</pre><h2>Only lower bound constraints<a name="2"></a></h2><pre class="codeinput">xsol = fminsearchbnd(rosen,[3 3],[2 2])
+</pre><pre class="codeoutput">
+xsol =
+
+            2            4
+
+</pre><h2>Only upper bound constraints<a name="3"></a></h2><pre class="codeinput">xsol = fminsearchbnd(rosen,[-5 -5],[],[0 0])
+</pre><pre class="codeoutput">
+xsol =
+
+  -1.0447e-13  -1.4451e-08
+
+</pre><h2>Dual constraints<a name="4"></a></h2><pre class="codeinput">xsol = fminsearchbnd(rosen,[2.5 2.5],[2 2],[3 3])
+</pre><pre class="codeoutput">
+xsol =
+
+            2            3
+
+</pre><h2>Mixed constraints<a name="5"></a></h2><pre class="codeinput">xsol = fminsearchbnd(rosen,[0 0],[2 -inf],[inf 3])
+</pre><pre class="codeoutput">
+xsol =
+
+            2            3
+
+</pre><h2>Provide your own fminsearch options<a name="6"></a></h2><pre class="codeinput">opts = optimset(<span class="string">'fminsearch'</span>);
+opts.Display = <span class="string">'iter'</span>;
+opts.TolX = 1.e-12;
+opts.MaxFunEvals = 100;
+
+n = [10,5];
+H = randn(n);
+H=H'*H;
+Quadraticfun = @(x) x*H*x';
+
+<span class="comment">% Global minimizer is at [0 0 0 0 0].</span>
+<span class="comment">% Set all lower bound constraints, all of which will</span>
+<span class="comment">% be active in this test.</span>
+LB = [.5 .5 .5 .5 .5];
+xsol = fminsearchbnd(Quadraticfun,[1 2 3 4 5],LB,[],opts)
+</pre><pre class="codeoutput"> 
+ Iteration   Func-count     min f(x)         Procedure
+     0            1          173.731         
+     1            6          172.028         initial simplex
+     2            8          162.698         expand
+     3            9          162.698         reflect
+     4           11          151.902         expand
+     5           13          138.235         expand
+     6           14          138.235         reflect
+     7           16          126.604         expand
+     8           17          126.604         reflect
+     9           19          97.3266         expand
+    10           20          97.3266         reflect
+    11           21          97.3266         reflect
+    12           22          97.3266         reflect
+    13           24          73.7178         expand
+    14           25          73.7178         reflect
+    15           26          73.7178         reflect
+    16           28          50.8236         expand
+    17           29          50.8236         reflect
+    18           31          41.6294         expand
+    19           33          30.4252         expand
+    20           34          30.4252         reflect
+    21           36           27.782         reflect
+    22           37           27.782         reflect
+    23           39           27.782         contract inside
+    24           41          22.6509         reflect
+    25           42          22.6509         reflect
+    26           43          22.6509         reflect
+    27           44          22.6509         reflect
+    28           45          22.6509         reflect
+    29           47          21.0211         reflect
+    30           48          21.0211         reflect
+    31           49          21.0211         reflect
+    32           51          21.0211         contract inside
+    33           52          21.0211         reflect
+    34           54          20.7613         contract inside
+    35           55          20.7613         reflect
+    36           56          20.7613         reflect
+    37           57          20.7613         reflect
+    38           59          20.6012         contract inside
+    39           61          20.5324         contract inside
+    40           63          20.4961         contract inside
+    41           65          20.3886         contract inside
+    42           67          20.2121         reflect
+    43           69          20.0876         contract inside
+    44           71          19.9164         reflect
+    45           72          19.9164         reflect
+    46           74          19.9164         contract inside
+    47           76          19.9164         contract outside
+    48           78          19.3349         expand
+    49           80          19.3349         contract inside
+    50           81          19.3349         reflect
+    51           82          19.3349         reflect
+    52           84          18.8721         expand
+    53           85          18.8721         reflect
+    54           87          18.6427         expand
+    55           89          17.4548         expand
+    56           90          17.4548         reflect
+    57           92          16.0113         expand
+    58           93          16.0113         reflect
+    59           94          16.0113         reflect
+    60           96          14.6134         expand
+    61           98          12.5445         expand
+    62           99          12.5445         reflect
+    63          101          10.7311         expand
+ 
+Exiting: Maximum number of function evaluations has been exceeded
+         - increase MaxFunEvals option.
+         Current function value: 10.731146 
+
+
+xsol =
+
+       1.7022       1.0787       1.2034       0.5006      0.64666
+
+</pre><h2>Exactly fix one variable, constrain some others, and set a tolerance<a name="7"></a></h2><pre class="codeinput">opts = optimset(<span class="string">'fminsearch'</span>);
+opts.TolFun = 1.e-12;
+
+LB = [-inf 2 1 -10];
+UB = [ inf  inf 1  inf];
+xsol = fminsearchbnd(@(x) norm(x),[1 3 1 1],LB,UB,opts)
+</pre><pre class="codeoutput">
+xsol =
+
+  -4.9034e-07            2            1   5.1394e-07
+
+</pre><h2>All the standard outputs from fminsearch are still returned<a name="8"></a></h2><pre class="codeinput">[xsol,fval,exitflag,output] = fminsearchbnd(@(x) norm(x),[1 3 1 1],LB,UB)
+</pre><pre class="codeoutput">
+xsol =
+
+   3.1094e-05            2            1  -5.1706e-05
+
+
+fval =
+
+       2.2361
+
+
+exitflag =
+
+     1
+
+
+output = 
+
+    iterations: 77
+     funcCount: 138
+     algorithm: 'Nelder-Mead simplex direct search'
+       message: [1x194 char]
+
+</pre><p class="footer"><br>
+         Published with MATLAB&reg; 7.0.1<br></p>
+      <!--
+##### SOURCE BEGIN #####
+%% Optimization of a simple (Rosenbrock) function, with no constraints
+rosen = @(x) (1-x(1)).^2 + 105*(x(2)-x(1).^2).^2;
+
+% With no constraints, operation simply passes through
+% directly to fminsearch. The solution should be [1 1]
+xsol = fminsearchbnd(rosen,[3 3])
+
+%% Only lower bound constraints
+xsol = fminsearchbnd(rosen,[3 3],[2 2])
+
+%% Only upper bound constraints
+xsol = fminsearchbnd(rosen,[-5 -5],[],[0 0])
+
+%% Dual constraints
+xsol = fminsearchbnd(rosen,[2.5 2.5],[2 2],[3 3])
+
+%% Mixed constraints
+xsol = fminsearchbnd(rosen,[0 0],[2 -inf],[inf 3])
+
+%% Provide your own fminsearch options
+opts = optimset('fminsearch');
+opts.Display = 'iter';
+opts.TolX = 1.e-12;
+opts.MaxFunEvals = 100;
+
+n = [10,5];
+H = randn(n);
+H=H'*H;
+Quadraticfun = @(x) x*H*x';
+
+% Global minimizer is at [0 0 0 0 0].
+% Set all lower bound constraints, all of which will
+% be active in this test.
+LB = [.5 .5 .5 .5 .5];
+xsol = fminsearchbnd(Quadraticfun,[1 2 3 4 5],LB,[],opts)
+
+%% Exactly fix one variable, constrain some others, and set a tolerance
+opts = optimset('fminsearch');
+opts.TolFun = 1.e-12;
+
+LB = [-inf 2 1 -10];
+UB = [ inf  inf 1  inf];
+xsol = fminsearchbnd(@(x) norm(x),[1 3 1 1],LB,UB,opts)
+
+%% All the standard outputs from fminsearch are still returned
+[xsol,fval,exitflag,output] = fminsearchbnd(@(x) norm(x),[1 3 1 1],LB,UB)
+
+
+##### SOURCE END #####
+-->
+   </body>
+</html>
\ No newline at end of file
diff --git a/Lib/various/FMINSEARCHBND/FMINSEARCHBND/demo/html/fminsearchcon_demo.html b/Lib/various/FMINSEARCHBND/FMINSEARCHBND/demo/html/fminsearchcon_demo.html
new file mode 100644
index 0000000..113f251
--- /dev/null
+++ b/Lib/various/FMINSEARCHBND/FMINSEARCHBND/demo/html/fminsearchcon_demo.html
@@ -0,0 +1,342 @@
+<html xmlns:mwsh="http://www.mathworks.com/namespace/mcode/v1/syntaxhighlight.dtd">
+   <head>
+      <meta http-equiv="Content-Type" content="text/html; charset=utf-8">
+   
+      <!--
+This HTML is auto-generated from an M-file.
+To make changes, update the M-file and republish this document.
+      -->
+      <title>fminsearchcon_demo</title>
+      <meta name="generator" content="MATLAB 7.0.1">
+      <meta name="date" content="2006-12-16">
+      <meta name="m-file" content="fminsearchcon_demo"><style>
+body {
+  background-color: white;
+  margin:10px;
+}
+h1 {
+  color: #990000; 
+  font-size: x-large;
+}
+h2 {
+  color: #990000;
+  font-size: medium;
+}
+p.footer {
+  text-align: right;
+  font-size: xx-small;
+  font-weight: lighter;
+  font-style: italic;
+  color: gray;
+}
+
+pre.codeinput {
+  margin-left: 30px;
+}
+
+span.keyword {color: #0000FF}
+span.comment {color: #228B22}
+span.string {color: #A020F0}
+span.untermstring {color: #B20000}
+span.syscmd {color: #B28C00}
+
+pre.showbuttons {
+  margin-left: 30px;
+  border: solid black 2px;
+  padding: 4px;
+  background: #EBEFF3;
+}
+
+pre.codeoutput {
+  color: gray;
+  font-style: italic;
+}
+pre.error {
+  color: red;
+}
+
+/* Make the text shrink to fit narrow windows, but not stretch too far in 
+wide windows.  On Gecko-based browsers, the shrink-to-fit doesn't work. */ 
+p,h1,h2,div {
+  /* for MATLAB's browser */
+  width: 600px;
+  /* for Mozilla, but the "width" tag overrides it anyway */
+  max-width: 600px;
+  /* for IE */
+  width:expression(document.body.clientWidth > 620 ? "600px": "auto" );
+}
+
+    </style></head>
+   <body>
+      <h2>Contents</h2>
+      <div>
+         <ul>
+            <li><a href="#1">Optimization of a simple (Rosenbrock) function</a></li>
+            <li><a href="#2">Only lower bound constraints</a></li>
+            <li><a href="#3">Only upper bound constraints</a></li>
+            <li><a href="#4">Dual constraints</a></li>
+            <li><a href="#5">Mixed constraints</a></li>
+            <li><a href="#6">Fix a variable as constant, x(2) == 3</a></li>
+            <li><a href="#7">Linear inequality, x(1) + x(2) &lt;= 1</a></li>
+            <li><a href="#8">Nonlinear inequality, norm(x) &lt;= 1</a></li>
+            <li><a href="#9">Minimize a linear objective, subject to a nonlinear constraints.</a></li>
+            <li><a href="#10">Provide your own fminsearch options</a></li>
+            <li><a href="#11">Exactly fix one variable, constrain some others, and set a tolerance</a></li>
+            <li><a href="#12">All the standard outputs from fminsearch are still returned</a></li>
+         </ul>
+      </div>
+      <h2>Optimization of a simple (Rosenbrock) function<a name="1"></a></h2><pre class="codeinput">rosen = @(x) (1-x(1)).^2 + 105*(x(2)-x(1).^2).^2;
+
+<span class="comment">% With no constraints, operation simply passes through</span>
+<span class="comment">% directly to fminsearch. The solution should be [1 1]</span>
+xsol = fminsearchcon(rosen,[3 3])
+</pre><pre class="codeoutput">
+xsol =
+
+      0.99998      0.99995
+
+</pre><h2>Only lower bound constraints<a name="2"></a></h2><pre class="codeinput">xsol = fminsearchcon(rosen,[3 3],[2 2])
+</pre><pre class="codeoutput">
+xsol =
+
+            2            4
+
+</pre><h2>Only upper bound constraints<a name="3"></a></h2><pre class="codeinput">xsol = fminsearchcon(rosen,[-5 -5],[],[0 0])
+</pre><pre class="codeoutput">
+xsol =
+
+  -1.0447e-13  -1.4451e-08
+
+</pre><h2>Dual constraints<a name="4"></a></h2><pre class="codeinput">xsol = fminsearchcon(rosen,[2.5 2.5],[2 2],[3 3])
+</pre><pre class="codeoutput">
+xsol =
+
+            2            3
+
+</pre><h2>Mixed constraints<a name="5"></a></h2><pre class="codeinput">xsol = fminsearchcon(rosen,[0 0],[2 -inf],[inf 3])
+</pre><pre class="codeoutput">
+xsol =
+
+            2            3
+
+</pre><h2>Fix a variable as constant, x(2) == 3<a name="6"></a></h2><pre class="codeinput">fminsearchcon(rosen,[3 3],[-inf 3],[inf,3])
+</pre><pre class="codeoutput">
+ans =
+
+       1.7314            3
+
+</pre><h2>Linear inequality, x(1) + x(2) &lt;= 1<a name="7"></a></h2><pre class="codeinput">fminsearchcon(rosen,[0 0],[],[],[1 1],1)
+</pre><pre class="codeoutput">
+ans =
+
+       0.6187       0.3813
+
+</pre><h2>Nonlinear inequality, norm(x) &lt;= 1<a name="8"></a></h2><pre class="codeinput">fminsearchcon(rosen,[0 0],[],[],[],[],@(x) norm(x) - 1)
+</pre><pre class="codeoutput">
+ans =
+
+      0.78633      0.61778
+
+</pre><h2>Minimize a linear objective, subject to a nonlinear constraints.<a name="9"></a></h2><pre class="codeinput">fun = @(x) x*[-2;1];
+nonlcon = @(x) [norm(x) - 1;sin(sum(x))];
+fminsearchcon(fun,[0 0],[],[],[],[],nonlcon)
+</pre><pre class="codeoutput">
+ans =
+
+      0.70707     -0.70713
+
+</pre><h2>Provide your own fminsearch options<a name="10"></a></h2><pre class="codeinput">opts = optimset(<span class="string">'fminsearch'</span>);
+opts.Display = <span class="string">'iter'</span>;
+opts.TolX = 1.e-12;
+opts.MaxFunEvals = 100;
+
+n = [10,5];
+H = randn(n);
+H=H'*H;
+Quadraticfun = @(x) x*H*x';
+
+<span class="comment">% Global minimizer is at [0 0 0 0 0].</span>
+<span class="comment">% Set all lower bound constraints, all of which will</span>
+<span class="comment">% be active in this test.</span>
+LB = [.5 .5 .5 .5 .5];
+xsol = fminsearchcon(Quadraticfun,[1 2 3 4 5],LB,[],[],[],[],opts)
+</pre><pre class="codeoutput"> 
+ Iteration   Func-count     min f(x)         Procedure
+     0            1          351.442         
+     1            6          351.442         initial simplex
+     2            8          296.007         expand
+     3            9          296.007         reflect
+     4           10          296.007         reflect
+     5           11          296.007         reflect
+     6           13          248.853         expand
+     7           15          209.119         expand
+     8           16          209.119         reflect
+     9           17          209.119         reflect
+    10           19          171.673         expand
+    11           21          132.166         expand
+    12           22          132.166         reflect
+    13           23          132.166         reflect
+    14           24          132.166         reflect
+    15           25          132.166         reflect
+    16           27          129.206         reflect
+    17           28          129.206         reflect
+    18           30          124.943         reflect
+    19           32          124.943         contract inside
+    20           34          123.809         reflect
+    21           36          107.233         expand
+    22           38          107.233         contract outside
+    23           39          107.233         reflect
+    24           41          98.5266         expand
+    25           42          98.5266         reflect
+    26           44          94.0621         expand
+    27           46          87.7403         expand
+    28           48          86.0587         reflect
+    29           49          86.0587         reflect
+    30           50          86.0587         reflect
+    31           51          86.0587         reflect
+    32           53          85.8138         reflect
+    33           55          85.8138         contract inside
+    34           57          73.0352         expand
+    35           59          73.0352         contract inside
+    36           60          73.0352         reflect
+    37           61          73.0352         reflect
+    38           62          73.0352         reflect
+    39           64          64.9347         expand
+    40           66          62.3724         expand
+    41           68          62.3126         reflect
+    42           69          62.3126         reflect
+    43           71          57.2349         reflect
+    44           73           49.999         expand
+    45           74           49.999         reflect
+    46           75           49.999         reflect
+    47           76           49.999         reflect
+    48           78          48.1326         reflect
+    49           79          48.1326         reflect
+    50           81          48.1326         contract inside
+    51           83          46.1196         reflect
+    52           85          46.1196         contract inside
+    53           87          43.1862         reflect
+    54           88          43.1862         reflect
+    55           90          43.1862         contract inside
+    56           91          43.1862         reflect
+    57           92          43.1862         reflect
+    58           94          42.6876         reflect
+    59           95          42.6876         reflect
+    60           97          42.6876         contract outside
+    61           99          41.6298         reflect
+    62          100          41.6298         reflect
+ 
+Exiting: Maximum number of function evaluations has been exceeded
+         - increase MaxFunEvals option.
+         Current function value: 41.629797 
+
+
+xsol =
+
+        1.652      0.54831       2.4744       1.2291      0.56971
+
+</pre><h2>Exactly fix one variable, constrain some others, and set a tolerance<a name="11"></a></h2><pre class="codeinput">opts = optimset(<span class="string">'fminsearch'</span>);
+opts.TolFun = 1.e-12;
+
+LB = [-inf 2 1 -10];
+UB = [ inf  inf 1  inf];
+xsol = fminsearchcon(@(x) norm(x),[1 3 1 1],LB,UB,[],[],[],opts)
+</pre><pre class="codeoutput">
+xsol =
+
+  -4.9034e-07            2            1   5.1394e-07
+
+</pre><h2>All the standard outputs from fminsearch are still returned<a name="12"></a></h2><pre class="codeinput">[xsol,fval,exitflag,output] = fminsearchcon(@(x) norm(x),[1 3 1 1],LB,UB)
+</pre><pre class="codeoutput">
+xsol =
+
+   3.1094e-05            2            1  -5.1706e-05
+
+
+fval =
+
+       2.2361
+
+
+exitflag =
+
+     1
+
+
+output = 
+
+    iterations: 77
+     funcCount: 138
+     algorithm: 'Nelder-Mead simplex direct search'
+       message: [1x194 char]
+
+</pre><p class="footer"><br>
+         Published with MATLAB&reg; 7.0.1<br></p>
+      <!--
+##### SOURCE BEGIN #####
+%% Optimization of a simple (Rosenbrock) function
+rosen = @(x) (1-x(1)).^2 + 105*(x(2)-x(1).^2).^2;
+
+% With no constraints, operation simply passes through
+% directly to fminsearch. The solution should be [1 1]
+xsol = fminsearchcon(rosen,[3 3])
+
+%% Only lower bound constraints
+xsol = fminsearchcon(rosen,[3 3],[2 2])
+
+%% Only upper bound constraints
+xsol = fminsearchcon(rosen,[-5 -5],[],[0 0])
+
+%% Dual constraints
+xsol = fminsearchcon(rosen,[2.5 2.5],[2 2],[3 3])
+
+%% Mixed constraints
+xsol = fminsearchcon(rosen,[0 0],[2 -inf],[inf 3])
+
+%% Fix a variable as constant, x(2) == 3
+fminsearchcon(rosen,[3 3],[-inf 3],[inf,3])
+
+%% Linear inequality, x(1) + x(2) <= 1
+fminsearchcon(rosen,[0 0],[],[],[1 1],1)
+
+%% Nonlinear inequality, norm(x) <= 1
+fminsearchcon(rosen,[0 0],[],[],[],[],@(x) norm(x) - 1)
+
+%% Minimize a linear objective, subject to a nonlinear constraints.
+fun = @(x) x*[-2;1];
+nonlcon = @(x) [norm(x) - 1;sin(sum(x))];
+fminsearchcon(fun,[0 0],[],[],[],[],nonlcon)
+
+%% Provide your own fminsearch options
+opts = optimset('fminsearch');
+opts.Display = 'iter';
+opts.TolX = 1.e-12;
+opts.MaxFunEvals = 100;
+
+n = [10,5];
+H = randn(n);
+H=H'*H;
+Quadraticfun = @(x) x*H*x';
+
+% Global minimizer is at [0 0 0 0 0].
+% Set all lower bound constraints, all of which will
+% be active in this test.
+LB = [.5 .5 .5 .5 .5];
+xsol = fminsearchcon(Quadraticfun,[1 2 3 4 5],LB,[],[],[],[],opts)
+
+%% Exactly fix one variable, constrain some others, and set a tolerance
+opts = optimset('fminsearch');
+opts.TolFun = 1.e-12;
+
+LB = [-inf 2 1 -10];
+UB = [ inf  inf 1  inf];
+xsol = fminsearchcon(@(x) norm(x),[1 3 1 1],LB,UB,[],[],[],opts)
+
+%% All the standard outputs from fminsearch are still returned
+[xsol,fval,exitflag,output] = fminsearchcon(@(x) norm(x),[1 3 1 1],LB,UB)
+
+
+##### SOURCE END #####
+-->
+   </body>
+</html>
\ No newline at end of file
diff --git a/Lib/various/FMINSEARCHBND/FMINSEARCHBND/fminsearchbnd.m b/Lib/various/FMINSEARCHBND/FMINSEARCHBND/fminsearchbnd.m
new file mode 100644
index 0000000..0448eae
--- /dev/null
+++ b/Lib/various/FMINSEARCHBND/FMINSEARCHBND/fminsearchbnd.m
@@ -0,0 +1,307 @@
+function [x,fval,exitflag,output] = fminsearchbnd(fun,x0,LB,UB,options,varargin)
+% FMINSEARCHBND: FMINSEARCH, but with bound constraints by transformation
+% usage: x=FMINSEARCHBND(fun,x0)
+% usage: x=FMINSEARCHBND(fun,x0,LB)
+% usage: x=FMINSEARCHBND(fun,x0,LB,UB)
+% usage: x=FMINSEARCHBND(fun,x0,LB,UB,options)
+% usage: x=FMINSEARCHBND(fun,x0,LB,UB,options,p1,p2,...)
+% usage: [x,fval,exitflag,output]=FMINSEARCHBND(fun,x0,...)
+% 
+% arguments:
+%  fun, x0, options - see the help for FMINSEARCH
+%
+%  LB - lower bound vector or array, must be the same size as x0
+%
+%       If no lower bounds exist for one of the variables, then
+%       supply -inf for that variable.
+%
+%       If no lower bounds at all, then LB may be left empty.
+%
+%       Variables may be fixed in value by setting the corresponding
+%       lower and upper bounds to exactly the same value.
+%
+%  UB - upper bound vector or array, must be the same size as x0
+%
+%       If no upper bounds exist for one of the variables, then
+%       supply +inf for that variable.
+%
+%       If no upper bounds at all, then UB may be left empty.
+%
+%       Variables may be fixed in value by setting the corresponding
+%       lower and upper bounds to exactly the same value.
+%
+% Notes:
+%
+%  If options is supplied, then TolX will apply to the transformed
+%  variables. All other FMINSEARCH parameters should be unaffected.
+%
+%  Variables which are constrained by both a lower and an upper
+%  bound will use a sin transformation. Those constrained by
+%  only a lower or an upper bound will use a quadratic
+%  transformation, and unconstrained variables will be left alone.
+%
+%  Variables may be fixed by setting their respective bounds equal.
+%  In this case, the problem will be reduced in size for FMINSEARCH.
+%
+%  The bounds are inclusive inequalities, which admit the
+%  boundary values themselves, but will not permit ANY function
+%  evaluations outside the bounds. These constraints are strictly
+%  followed.
+%
+%  If your problem has an EXCLUSIVE (strict) constraint which will
+%  not admit evaluation at the bound itself, then you must provide
+%  a slightly offset bound. An example of this is a function which
+%  contains the log of one of its parameters. If you constrain the
+%  variable to have a lower bound of zero, then FMINSEARCHBND may
+%  try to evaluate the function exactly at zero.
+%
+%
+% Example usage:
+% rosen = @(x) (1-x(1)).^2 + 105*(x(2)-x(1).^2).^2;
+%
+% fminsearch(rosen,[3 3])     % unconstrained
+% ans =
+%    1.0000    1.0000
+%
+% fminsearchbnd(rosen,[3 3],[2 2],[])     % constrained
+% ans =
+%    2.0000    4.0000
+%
+% See test_main.m for other examples of use.
+%
+%
+% See also: fminsearch, fminspleas
+%
+%
+% Author: John D'Errico
+% E-mail: woodchips@rochester.rr.com
+% Release: 4
+% Release date: 7/23/06
+
+% size checks
+xsize = size(x0);
+x0 = x0(:);
+n=length(x0);
+
+if (nargin<3) || isempty(LB)
+  LB = repmat(-inf,n,1);
+else
+  LB = LB(:);
+end
+if (nargin<4) || isempty(UB)
+  UB = repmat(inf,n,1);
+else
+  UB = UB(:);
+end
+
+if (n~=length(LB)) || (n~=length(UB))
+  error 'x0 is incompatible in size with either LB or UB.'
+end
+
+% set default options if necessary
+if (nargin<5) || isempty(options)
+  options = optimset('fminsearch');
+end
+
+% stuff into a struct to pass around
+params.args = varargin;
+params.LB = LB;
+params.UB = UB;
+params.fun = fun;
+params.n = n;
+% note that the number of parameters may actually vary if 
+% a user has chosen to fix one or more parameters
+params.xsize = xsize;
+params.OutputFcn = [];
+
+% 0 --> unconstrained variable
+% 1 --> lower bound only
+% 2 --> upper bound only
+% 3 --> dual finite bounds
+% 4 --> fixed variable
+params.BoundClass = zeros(n,1);
+for i=1:n
+  k = isfinite(LB(i)) + 2*isfinite(UB(i));
+  params.BoundClass(i) = k;
+  if (k==3) && (LB(i)==UB(i))
+    params.BoundClass(i) = 4;
+  end
+end
+
+% transform starting values into their unconstrained
+% surrogates. Check for infeasible starting guesses.
+x0u = x0;
+k=1;
+for i = 1:n
+  switch params.BoundClass(i)
+    case 1
+      % lower bound only
+      if x0(i)<=LB(i)
+        % infeasible starting value. Use bound.
+        x0u(k) = 0;
+      else
+        x0u(k) = sqrt(x0(i) - LB(i));
+      end
+      
+      % increment k
+      k=k+1;
+    case 2
+      % upper bound only
+      if x0(i)>=UB(i)
+        % infeasible starting value. use bound.
+        x0u(k) = 0;
+      else
+        x0u(k) = sqrt(UB(i) - x0(i));
+      end
+      
+      % increment k
+      k=k+1;
+    case 3
+      % lower and upper bounds
+      if x0(i)<=LB(i)
+        % infeasible starting value
+        x0u(k) = -pi/2;
+      elseif x0(i)>=UB(i)
+        % infeasible starting value
+        x0u(k) = pi/2;
+      else
+        x0u(k) = 2*(x0(i) - LB(i))/(UB(i)-LB(i)) - 1;
+        % shift by 2*pi to avoid problems at zero in fminsearch
+        % otherwise, the initial simplex is vanishingly small
+        x0u(k) = 2*pi+asin(max(-1,min(1,x0u(k))));
+      end
+      
+      % increment k
+      k=k+1;
+    case 0
+      % unconstrained variable. x0u(i) is set.
+      x0u(k) = x0(i);
+      
+      % increment k
+      k=k+1;
+    case 4
+      % fixed variable. drop it before fminsearch sees it.
+      % k is not incremented for this variable.
+  end
+  
+end
+% if any of the unknowns were fixed, then we need to shorten
+% x0u now.
+if k<=n
+  x0u(k:n) = [];
+end
+
+% were all the variables fixed?
+if isempty(x0u)
+  % All variables were fixed. quit immediately, setting the
+  % appropriate parameters, then return.
+  
+  % undo the variable transformations into the original space
+  x = xtransform(x0u,params);
+  
+  % final reshape
+  x = reshape(x,xsize);
+  
+  % stuff fval with the final value
+  fval = feval(params.fun,x,params.args{:});
+  
+  % fminsearchbnd was not called
+  exitflag = 0;
+  
+  output.iterations = 0;
+  output.funcCount = 1;
+  output.algorithm = 'fminsearch';
+  output.message = 'All variables were held fixed by the applied bounds';
+  
+  % return with no call at all to fminsearch
+  return
+end
+
+% Check for an outputfcn. If there is any, then substitute my
+% own wrapper function.
+if ~isempty(options.OutputFcn)
+  params.OutputFcn = options.OutputFcn;
+  options.OutputFcn = @outfun_wrapper;
+end
+
+% now we can call fminsearch, but with our own
+% intra-objective function.
+[xu,fval,exitflag,output] = fminsearch(@intrafun,x0u,options,params);
+
+% undo the variable transformations into the original space
+x = xtransform(xu,params);
+
+% final reshape to make sure the result has the proper shape
+x = reshape(x,xsize);
+
+% Use a nested function as the OutputFcn wrapper
+  function stop = outfun_wrapper(x,varargin);
+    % we need to transform x first
+    xtrans = xtransform(x,params);
+    
+    % then call the user supplied OutputFcn
+    stop = params.OutputFcn(xtrans,varargin{1:(end-1)});
+    
+  end
+
+end % mainline end
+
+% ======================================
+% ========= begin subfunctions =========
+% ======================================
+function fval = intrafun(x,params)
+% transform variables, then call original function
+
+% transform
+xtrans = xtransform(x,params);
+
+% and call fun
+fval = feval(params.fun,reshape(xtrans,params.xsize),params.args{:});
+
+end % sub function intrafun end
+
+% ======================================
+function xtrans = xtransform(x,params)
+% converts unconstrained variables into their original domains
+
+xtrans = zeros(params.xsize);
+% k allows some variables to be fixed, thus dropped from the
+% optimization.
+k=1;
+for i = 1:params.n
+  switch params.BoundClass(i)
+    case 1
+      % lower bound only
+      xtrans(i) = params.LB(i) + x(k).^2;
+      
+      k=k+1;
+    case 2
+      % upper bound only
+      xtrans(i) = params.UB(i) - x(k).^2;
+      
+      k=k+1;
+    case 3
+      % lower and upper bounds
+      xtrans(i) = (sin(x(k))+1)/2;
+      xtrans(i) = xtrans(i)*(params.UB(i) - params.LB(i)) + params.LB(i);
+      % just in case of any floating point problems
+      xtrans(i) = max(params.LB(i),min(params.UB(i),xtrans(i)));
+      
+      k=k+1;
+    case 4
+      % fixed variable, bounds are equal, set it at either bound
+      xtrans(i) = params.LB(i);
+    case 0
+      % unconstrained variable.
+      xtrans(i) = x(k);
+      
+      k=k+1;
+  end
+end
+
+end % sub function xtransform end
+
+
+
+
+
diff --git a/Lib/various/FMINSEARCHBND/FMINSEARCHBND/fminsearchcon.m b/Lib/various/FMINSEARCHBND/FMINSEARCHBND/fminsearchcon.m
new file mode 100644
index 0000000..a562e11
--- /dev/null
+++ b/Lib/various/FMINSEARCHBND/FMINSEARCHBND/fminsearchcon.m
@@ -0,0 +1,428 @@
+function [x,fval,exitflag,output]=fminsearchcon(fun,x0,LB,UB,A,b,nonlcon,options,varargin)
+% FMINSEARCHCON: Extension of FMINSEARCHBND with general inequality constraints
+% usage: x=FMINSEARCHCON(fun,x0)
+% usage: x=FMINSEARCHCON(fun,x0,LB)
+% usage: x=FMINSEARCHCON(fun,x0,LB,UB)
+% usage: x=FMINSEARCHCON(fun,x0,LB,UB,A,b)
+% usage: x=FMINSEARCHCON(fun,x0,LB,UB,A,b,nonlcon)
+% usage: x=FMINSEARCHCON(fun,x0,LB,UB,A,b,nonlcon,options)
+% usage: x=FMINSEARCHCON(fun,x0,LB,UB,A,b,nonlcon,options,p1,p2,...)
+% usage: [x,fval,exitflag,output]=FMINSEARCHCON(fun,x0,...)
+% 
+% arguments:
+%  fun, x0, options - see the help for FMINSEARCH
+%
+%       x0 MUST be a feasible point for the linear and nonlinear
+%       inequality constraints. If it is not inside the bounds
+%       then it will be moved to the nearest bound. If x0 is
+%       infeasible for the general constraints, then an error will
+%       be returned.
+%
+%  LB - lower bound vector or array, must be the same size as x0
+%
+%       If no lower bounds exist for one of the variables, then
+%       supply -inf for that variable.
+%
+%       If no lower bounds at all, then LB may be left empty.
+%
+%       Variables may be fixed in value by setting the corresponding
+%       lower and upper bounds to exactly the same value.
+%
+%  UB - upper bound vector or array, must be the same size as x0
+%
+%       If no upper bounds exist for one of the variables, then
+%       supply +inf for that variable.
+%
+%       If no upper bounds at all, then UB may be left empty.
+%
+%       Variables may be fixed in value by setting the corresponding
+%       lower and upper bounds to exactly the same value.
+%
+%  A,b - (OPTIONAL) Linear inequality constraint array and right
+%       hand side vector. (Note: these constraints were chosen to
+%       be consistent with those of fmincon.)
+%
+%       A*x <= b
+%
+%  nonlcon - (OPTIONAL) general nonlinear inequality constraints
+%       NONLCON must return a set of general inequality constraints.
+%       These will be enforced such that nonlcon is always <= 0.
+%
+%       nonlcon(x) <= 0
+%
+%
+% Notes:
+%
+%  If options is supplied, then TolX will apply to the transformed
+%  variables. All other FMINSEARCH parameters should be unaffected.
+%
+%  Variables which are constrained by both a lower and an upper
+%  bound will use a sin transformation. Those constrained by
+%  only a lower or an upper bound will use a quadratic
+%  transformation, and unconstrained variables will be left alone.
+%
+%  Variables may be fixed by setting their respective bounds equal.
+%  In this case, the problem will be reduced in size for FMINSEARCH.
+%
+%  The bounds are inclusive inequalities, which admit the
+%  boundary values themselves, but will not permit ANY function
+%  evaluations outside the bounds. These constraints are strictly
+%  followed.
+%
+%  If your problem has an EXCLUSIVE (strict) constraint which will
+%  not admit evaluation at the bound itself, then you must provide
+%  a slightly offset bound. An example of this is a function which
+%  contains the log of one of its parameters. If you constrain the
+%  variable to have a lower bound of zero, then FMINSEARCHCON may
+%  try to evaluate the function exactly at zero.
+%
+%  Inequality constraints are enforced with an implicit penalty
+%  function approach. But the constraints are tested before
+%  any function evaluations are ever done, so the actual objective
+%  function is NEVER evaluated outside of the feasible region.
+%
+%
+% Example usage:
+% rosen = @(x) (1-x(1)).^2 + 105*(x(2)-x(1).^2).^2;
+%
+% Fully unconstrained problem
+% fminsearchcon(rosen,[3 3])
+% ans =
+%    1.0000    1.0000
+%
+% lower bound constrained
+% fminsearchcon(rosen,[3 3],[2 2],[])
+% ans =
+%    2.0000    4.0000
+%
+% x(2) fixed at 3
+% fminsearchcon(rosen,[3 3],[-inf 3],[inf,3])
+% ans =
+%    1.7314    3.0000
+%
+% simple linear inequality: x(1) + x(2) <= 1
+% fminsearchcon(rosen,[0 0],[],[],[1 1],.5)
+% 
+% ans =
+%    0.6187    0.3813
+% 
+% general nonlinear inequality: sqrt(x(1)^2 + x(2)^2) <= 1
+% fminsearchcon(rosen,[0 0],[],[],[],[],@(x) norm(x)-1)
+% ans =
+%    0.78633   0.61778
+%
+% Of course, any combination of the above constraints is
+% also possible.
+%
+% See test_main.m for other examples of use.
+%
+%
+% See also: fminsearch, fminspleas, fminsearchbnd
+%
+%
+% Author: John D'Errico
+% E-mail: woodchips@rochester.rr.com
+% Release: 1.0
+% Release date: 12/16/06
+
+% size checks
+xsize = size(x0);
+x0 = x0(:);
+n=length(x0);
+
+if (nargin<3) || isempty(LB)
+  LB = repmat(-inf,n,1);
+else
+  LB = LB(:);
+end
+if (nargin<4) || isempty(UB)
+  UB = repmat(inf,n,1);
+else
+  UB = UB(:);
+end
+
+if (n~=length(LB)) || (n~=length(UB))
+  error 'x0 is incompatible in size with either LB or UB.'
+end
+
+% defaults for A,b
+if (nargin<5) || isempty(A)
+  A = [];
+end
+if (nargin<6) || isempty(b)
+  b = [];
+end
+nA = [];
+nb = [];
+if (isempty(A)&&~isempty(b)) || (isempty(b)&&~isempty(A)) 
+  error 'Sizes of A and b are incompatible'
+elseif ~isempty(A)
+  nA = size(A);
+  b = b(:);
+  nb = size(b,1);
+  if nA(1)~=nb  
+    error 'Sizes of A and b are incompatible'
+  end
+  if nA(2)~=n
+    error 'A is incompatible in size with x0'
+  end
+end
+
+% defaults for nonlcon
+if (nargin<7) || isempty(nonlcon)
+  nonlcon = [];
+end
+
+% test for feasibility of the initial value
+% against any general inequality constraints
+if ~isempty(A)
+  if any(A*x0>b)
+    error 'Infeasible starting values (linear inequalities failed).'
+  end
+end
+if ~isempty(nonlcon)
+  if any(feval(nonlcon,(reshape(x0,xsize)),varargin{:})>0)
+    error 'Infeasible starting values (nonlinear inequalities failed).'
+  end
+end
+
+% set default options if necessary
+if (nargin<8) || isempty(options)
+  options = optimset('fminsearch');
+end
+
+% stuff into a struct to pass around
+params.args = varargin;
+params.LB = LB;
+params.UB = UB;
+params.fun = fun;
+params.n = n;
+params.xsize = xsize;
+
+params.OutputFcn = [];
+
+params.A = A;
+params.b = b;
+params.nonlcon = nonlcon;
+
+% 0 --> unconstrained variable
+% 1 --> lower bound only
+% 2 --> upper bound only
+% 3 --> dual finite bounds
+% 4 --> fixed variable
+params.BoundClass = zeros(n,1);
+for i=1:n
+  k = isfinite(LB(i)) + 2*isfinite(UB(i));
+  params.BoundClass(i) = k;
+  if (k==3) && (LB(i)==UB(i))
+    params.BoundClass(i) = 4;
+  end
+end
+
+% transform starting values into their unconstrained
+% surrogates. Check for infeasible starting guesses.
+x0u = x0;
+k=1;
+for i = 1:n
+  switch params.BoundClass(i)
+    case 1
+      % lower bound only
+      if x0(i)<=LB(i)
+        % infeasible starting value. Use bound.
+        x0u(k) = 0;
+      else
+        x0u(k) = sqrt(x0(i) - LB(i));
+      end
+      
+      % increment k
+      k=k+1;
+    case 2
+      % upper bound only
+      if x0(i)>=UB(i)
+        % infeasible starting value. use bound.
+        x0u(k) = 0;
+      else
+        x0u(k) = sqrt(UB(i) - x0(i));
+      end
+      
+      % increment k
+      k=k+1;
+    case 3
+      % lower and upper bounds
+      if x0(i)<=LB(i)
+        % infeasible starting value
+        x0u(k) = -pi/2;
+      elseif x0(i)>=UB(i)
+        % infeasible starting value
+        x0u(k) = pi/2;
+      else
+        x0u(k) = 2*(x0(i) - LB(i))/(UB(i)-LB(i)) - 1;
+        % shift by 2*pi to avoid problems at zero in fminsearch
+        % otherwise, the initial simplex is vanishingly small
+        x0u(k) = 2*pi+asin(max(-1,min(1,x0u(k))));
+      end
+      
+      % increment k
+      k=k+1;
+    case 0
+      % unconstrained variable. x0u(i) is set.
+      x0u(k) = x0(i);
+      
+      % increment k
+      k=k+1;
+    case 4
+      % fixed variable. drop it before fminsearch sees it.
+      % k is not incremented for this variable.
+  end
+  
+end
+% if any of the unknowns were fixed, then we need to shorten
+% x0u now.
+if k<=n
+  x0u(k:n) = [];
+end
+
+% were all the variables fixed?
+if isempty(x0u)
+  % All variables were fixed. quit immediately, setting the
+  % appropriate parameters, then return.
+  
+  % undo the variable transformations into the original space
+  x = xtransform(x0u,params);
+  
+  % final reshape
+  x = reshape(x,xsize);
+  
+  % stuff fval with the final value
+  fval = feval(params.fun,x,params.args{:});
+  
+  % fminsearchbnd was not called
+  exitflag = 0;
+  
+  output.iterations = 0;
+  output.funcCount = 1;
+  output.algorithm = 'fminsearch';
+  output.message = 'All variables were held fixed by the applied bounds';
+  
+  % return with no call at all to fminsearch
+  return
+end
+
+% Check for an outputfcn. If there is any, then substitute my
+% own wrapper function.
+if ~isempty(options.OutputFcn)
+  params.OutputFcn = options.OutputFcn;
+  options.OutputFcn = @outfun_wrapper;
+end
+
+% now we can call fminsearch, but with our own
+% intra-objective function.
+[xu,fval,exitflag,output] = fminsearch(@intrafun,x0u,options,params);
+
+% undo the variable transformations into the original space
+x = xtransform(xu,params);
+
+% final reshape
+x = reshape(x,xsize);
+
+% Use a nested function as the OutputFcn wrapper
+  function stop = outfun_wrapper(x,varargin);
+    % we need to transform x first
+    xtrans = xtransform(x,params);
+    
+    % then call the user supplied OutputFcn
+    stop = params.OutputFcn(xtrans,varargin{1:(end-1)});
+    
+  end
+
+end % mainline end
+
+% ======================================
+% ========= begin subfunctions =========
+% ======================================
+function fval = intrafun(x,params)
+% transform variables, test constraints, then call original function
+
+% transform
+xtrans = xtransform(x,params);
+
+% test constraints before the function call
+
+% First, do the linear inequality constraints, if any
+if ~isempty(params.A)
+  % Required: A*xtrans <= b
+  if any(params.A*xtrans(:) > params.b)
+    % linear inequality constraints failed. Just return inf.
+    fval = inf;
+    return
+  end
+end
+
+% resize xtrans to be the correct size for the nonlcon
+% and objective function calls
+xtrans = reshape(xtrans,params.xsize);
+
+% Next, do the nonlinear inequality constraints
+if ~isempty(params.nonlcon)
+  % Required: nonlcon(xtrans) <= 0
+  cons = feval(params.nonlcon,xtrans,params.args{:});
+  if any(cons(:) > 0)
+    % nonlinear inequality constraints failed. Just return inf.
+    fval = inf;
+    return
+  end
+end
+
+% we survived the general inequality constraints. Only now
+% do we evaluate the objective function.
+
+% append any additional parameters to the argument list
+fval = feval(params.fun,xtrans,params.args{:});
+
+end % sub function intrafun end
+
+% ======================================
+function xtrans = xtransform(x,params)
+% converts unconstrained variables into their original domains
+
+xtrans = zeros(1,params.n);
+% k allows some variables to be fixed, thus dropped from the
+% optimization.
+k=1;
+for i = 1:params.n
+  switch params.BoundClass(i)
+    case 1
+      % lower bound only
+      xtrans(i) = params.LB(i) + x(k).^2;
+      
+      k=k+1;
+    case 2
+      % upper bound only
+      xtrans(i) = params.UB(i) - x(k).^2;
+      
+      k=k+1;
+    case 3
+      % lower and upper bounds
+      xtrans(i) = (sin(x(k))+1)/2;
+      xtrans(i) = xtrans(i)*(params.UB(i) - params.LB(i)) + params.LB(i);
+      % just in case of any floating point problems
+      xtrans(i) = max(params.LB(i),min(params.UB(i),xtrans(i)));
+      
+      k=k+1;
+    case 4
+      % fixed variable, bounds are equal, set it at either bound
+      xtrans(i) = params.LB(i);
+    case 0
+      % unconstrained variable.
+      xtrans(i) = x(k);
+      
+      k=k+1;
+  end
+end
+
+end % sub function xtransform end
+
+
+
+
+
diff --git a/Lib/various/FMINSEARCHBND/FMINSEARCHBND/test/test_main.m b/Lib/various/FMINSEARCHBND/FMINSEARCHBND/test/test_main.m
new file mode 100644
index 0000000..404141c
--- /dev/null
+++ b/Lib/various/FMINSEARCHBND/FMINSEARCHBND/test/test_main.m
@@ -0,0 +1,53 @@
+%% Optimization of a simple (Rosenbrock) function, with no constraints
+% The unconstrained solution is at [1,1]
+rosen = @(x) (1-x(1)).^2 + 105*(x(2)-x(1).^2).^2;
+
+% With no constraints, operation simply passes through
+% directly to fminsearch. The solution should be [1 1]
+xsol = fminsearchbnd(rosen,[3 3])
+
+%% Full lower and upper bound constraints which will all be inactive
+xsol = fminsearchbnd(rosen,[3 3],[-1 -1],[4 4])
+
+%% Only lower bound constraints
+xsol = fminsearchbnd(rosen,[3 3],[2 2])
+
+%% Only upper bound constraints
+xsol = fminsearchbnd(rosen,[-5 -5],[],[0 0])
+
+%% Dual constraints
+xsol = fminsearchbnd(rosen,[2.5 2.5],[2 2],[3 3])
+
+%% Dual constraints, with an infeasible starting guess
+xsol = fminsearchbnd(rosen,[0 0],[2 2],[3 3])
+
+%% Mixed constraints
+xsol = fminsearchbnd(rosen,[0 0],[2 -inf],[inf 3])
+
+%% Provide your own fminsearch options
+opts = optimset('fminsearch');
+opts.Display = 'iter';
+opts.TolX = 1.e-12;
+
+n = [10,5];
+H = randn(n);
+H=H'*H;
+Quadraticfun = @(x) x*H*x';
+
+% Global minimizer is at [0 0 0 0 0].
+% Set all lower bound constraints, all of which will
+% be active in this test.
+LB = [.5 .5 .5 .5 .5];
+xsol = fminsearchbnd(Quadraticfun,[1 2 3 4 5],LB,[],opts)
+
+%% Exactly fix one variable, constrain some others, and set a tolerance
+opts = optimset('fminsearch');
+opts.TolFun = 1.e-12;
+
+LB = [-inf 2 1 -10];
+UB = [ inf  inf 1  inf];
+xsol = fminsearchbnd(@(x) norm(x),[1 3 1 1],LB,UB,opts)
+
+%% All the standard outputs from fminsearch are still returned
+[xsol,fval,exitflag,output] = fminsearchbnd(@(x) norm(x),[1 3 1 1],LB,UB)
+
diff --git a/Lib/various/FMINSEARCHBND/license.txt b/Lib/various/FMINSEARCHBND/license.txt
new file mode 100644
index 0000000..a58a4fe
--- /dev/null
+++ b/Lib/various/FMINSEARCHBND/license.txt
@@ -0,0 +1,24 @@
+Copyright (c) 2006, John D'Errico
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are
+met:
+
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * 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
+
+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.
diff --git a/Lib/various/gen_meas.m b/Lib/various/gen_meas.m
new file mode 100644
index 0000000..7f5e435
--- /dev/null
+++ b/Lib/various/gen_meas.m
@@ -0,0 +1,35 @@
+%%
+function x_out = gen_meas(params,x,Npoint,sigma)    
+             
+    NSeg = length(params.input_data.SOC)-1;
+    NPointsSeg = floor(Npoint/NSeg);
+    
+    % initialize state
+    x_out = zeros(length(x),NSeg*NPointsSeg);
+    
+    % create cloud
+    for i=1:NSeg
+        for j=1:NPointsSeg
+            % generate SOC        
+            x_out(1,(i-1)*NPointsSeg+j) =  unifrnd(params.input_data.SOC(i),params.input_data.SOC(i+1));            
+            
+            % generate points (noiseless)
+            ref(3) = interp1(params.input_data.SOC(i:i+1), params.input_data.OCV(i:i+1), x_out(1,(i-1)*NPointsSeg+j));
+            ref(4) = interp1(params.input_data.SOC(i:i+1), params.input_data.R0(i:i+1), x_out(1,(i-1)*NPointsSeg+j));
+            ref(5) = interp1(params.input_data.SOC(i:i+1), params.input_data.R1(i:i+1), x_out(1,(i-1)*NPointsSeg+j));
+            ref(6) = interp1(params.input_data.SOC(i:i+1), params.input_data.C1(i:i+1), x_out(1,(i-1)*NPointsSeg+j));
+            
+            x_out(3,(i-1)*NPointsSeg+j) = ref(3)*(1+sigma*randn);
+            x_out(4,(i-1)*NPointsSeg+j) = ref(4)*(1+sigma*randn);
+            x_out(5,(i-1)*NPointsSeg+j) = ref(5)*(1+sigma*randn);
+            x_out(6,(i-1)*NPointsSeg+j) = ref(6)*(1+sigma*randn);
+        
+        end
+    end  
+
+    [x_out(1,:), I] = sort(x_out(1,:));
+    x_out(3,:) = x_out(3,I);
+    x_out(4,:) = x_out(4,I);
+    x_out(5,:) = x_out(5,I);
+    x_out(6,:) = x_out(6,I);
+end
\ No newline at end of file
diff --git a/Lib/various/odehybrid/.gitattributes b/Lib/various/odehybrid/.gitattributes
new file mode 100644
index 0000000..bdb0cab
--- /dev/null
+++ b/Lib/various/odehybrid/.gitattributes
@@ -0,0 +1,17 @@
+# Auto detect text files and perform LF normalization
+* text=auto
+
+# Custom for Visual Studio
+*.cs     diff=csharp
+
+# Standard to msysgit
+*.doc	 diff=astextplain
+*.DOC	 diff=astextplain
+*.docx diff=astextplain
+*.DOCX diff=astextplain
+*.dot  diff=astextplain
+*.DOT  diff=astextplain
+*.pdf  diff=astextplain
+*.PDF	 diff=astextplain
+*.rtf	 diff=astextplain
+*.RTF	 diff=astextplain
diff --git a/Lib/various/odehybrid/README.md b/Lib/various/odehybrid/README.md
new file mode 100644
index 0000000..0ff6af0
--- /dev/null
+++ b/Lib/various/odehybrid/README.md
@@ -0,0 +1,11 @@
+![odehybrid](http://www.anuncommonlab.com/odehybrid/img/odehybrid600.png)
+
+The odehybrid library makes it easy to create simulations of dynamical systems with both continuous-time components (such as physical models) and multiple discrete-time components (such as digital controllers and signal processors). It's like MATLAB's built-in ode45 function, but extended for discrete-time systems, complex state organization, and logging. 
+To get started, see examples_odehybrid.m. 
+View online documentation:
+
+http://www.anuncommonlab.com/doc/odehybrid/
+
+Homepage:
+
+http://www.anuncommonlab.com/odehybrid/
diff --git a/Lib/various/odehybrid/TimeSeriesLogger.m b/Lib/various/odehybrid/TimeSeriesLogger.m
new file mode 100644
index 0000000..96ae79d
--- /dev/null
+++ b/Lib/various/odehybrid/TimeSeriesLogger.m
@@ -0,0 +1,310 @@
+classdef TimeSeriesLogger < handle
+
+% TimeSeriesLogger class
+% 
+% A class for keeping track of numerous pieces of data over time throughout
+% a process. It's designed to be easy to use and relatively quickly. It
+% logs numeric types of any size (scalars, vectors, or matrices), as long
+% as the size is consistent from sample to sample.
+% 
+% Example:
+% 
+%   log = TimeSeriesLogger(); % Make a new logger.
+%   x   = [0; 0];
+%   for t = 1:100             % Make a random walk.
+%     x = x + randn(2, 1);    % Take a single step.
+%     log.add('walk', t, x);  % Log a single sample, x, at time t.
+%   end
+%   log.plot();               % Plot everything.
+% 
+% We can also access specific logs by their names. In the above, we only
+% have one log ('walk'). Let's get that log from the logger.
+% 
+%   [t_log, x_log] = log.get_log('walk'); % Get a specific log.
+%   plot(x_log(:, 1), x_log(:, 2));       % Do something with it.
+%
+% We can make sure a log exists before trying to do anything with it:
+% 
+%   if log.contains('walk')
+%     x_log = log.get_log('walk');
+%     plot(x_log(:, 1), x_log(:, 2));
+%   end
+%
+% If we want to log something but don't want it plotted when we call
+% |plot|, then we can pass in false to the logger when we add the signal.
+% 
+%   log.add('var1', t, x, false);
+% 
+% To group items into different figures when they're plotted, give them
+% "groups":
+% 
+%   log.add('var1', t, x, true, 'group1');
+%   log.add('var2', t, y, true, 'group2');
+%   log.add('foo',  t, bar, true, 'group1');
+% 
+% All of the items with common group names will be added to the same
+% figures as subplots.
+%
+% Finally, we can clear out the logger with |initialize|. This deletes all
+% data and returns the logger to its initial state.
+%
+%   log.initialize();
+% 
+% For more information on the various methods, see the individual methods
+% with |doc TimeSeriesLogger|:
+% 
+% <a href="matlab:doc TimeSeriesLogger;">doc TimeSeriesLogger</a>
+% 
+% Methods
+% -------
+% 
+% initialize  Clear out all data.
+% add         Add a single data point to a time series.
+% contains    See if a time series exists (by name).
+% get_log     Return a log by name.
+% plot        Plot the logged series.
+%
+% Notes
+% -----
+%
+% TimeSeriesLogger was created as the logging mechanism for odehybrid.
+% However, it's not dependent on that project and so can be used anywhere.
+%  
+% TimeSeriesLogger is not related to the 'timeseries' class in MATLAB.
+%
+% Since this is such a simple class, all properties are public, preventing
+% the need for getters and setters. However, under normal use cases, there
+% would be no reason to access any properties of the class.
+% 
+% Since this class never knows how much more data is going to be logged, it
+% can't preallocate the appropriate amount of space. However, increasing
+% the size of its data stores by one row every time a new sample is added
+% is very slow. To combat this, the data store starts off small and doubles
+% whenever the store is at capacity. Say we're logging a 4x1 signal. When
+% the first sample is added (say it's log k), data{k}{2} will be 1x4. When
+% the second signal is added, it becomes 2x4. For the third, it's 4x4, then
+% 8x4, 16x4, etc. A separate counter stores how much of this allocated 
+% space is currently used. This reduces the number of allocations from n to
+% log2(n). Practically, it saves a little time during logging without too
+% much complexity.
+%
+% Online doc: http://www.anuncommonlab.com/doc/odehybrid/TimeSeriesLogger.html
+%
+% Copyright 2014 An Uncommon Lab
+
+    properties
+        names = {}; % Unique identifier (within the group)
+        data  = {}; % Data, stored as {t, x}
+        show  = {}; % True iff the data should be plotted
+        sizes = {}; % Amount of allocated space
+        count = {}; % Amount of allocated space currently used
+        group = {}; % Plot group identifier
+    end
+    
+    methods
+        
+        function initialize(this)
+        % Clear everything out and start fresh.
+            this.names = {};
+            this.data  = {};
+            this.show  = {};
+            this.sizes = {};
+            this.count = {};
+            this.group = {};
+        end
+        
+        function added_to_list = add(this, name, t, x, show_it, group)
+        % Add a new log or append to an existing log by name.
+        % 
+        % log.add('var1', t, x);
+        % log.add('var1', t, x, true);  % Show log in plots (default)
+        % log.add('var1', t, x, false); % Don't show log in plots
+        % 
+        % % Signals can be grouped together into figures by given them
+        % a common group argument. Here, both var1 and var2 logs will
+        % be plotted together.
+        % log.add('var1', t,  x, true, 'group1');
+        % log.add('var2', t2, y, true, 'group1');
+        % 
+        % Returns true iff a new log was created.
+        %
+        % The signals are stored as time-data pairs, {t, x}, where each
+        % row of x corresponds to each row of t. So data{k}{1} contains
+        % the logged times for the kth signal, and data{k}{2} contains
+        % the data for the logged signal.
+            
+            % Defaults
+            if nargin < 5, show_it = true; end; % Plot by default.
+            if nargin < 6, group = '';     end; % Default group is ''.
+            
+            % See if we've already added this variable.
+            index = find(strcmp(this.names, name));
+            
+            % We'll add it to the list if it's not there (and we return
+            % this status).
+            added_to_list = isempty(index);
+            
+            % If not, add it to the list.
+            if added_to_list
+                
+                this.data{end+1}  = {t, x(:).'};
+                this.names{end+1} = name;
+                this.show{end+1}  = show_it;
+                this.sizes{end+1} = 1;
+                this.count{end+1} = 1;
+                this.group{end+1} = group;
+                
+            % Otherwise, append.
+            else
+                
+                % If we've used up all reserved space, double it.
+                c = this.count{index} + 1;
+                if c > this.sizes{index}
+                    this.data{index}{1} = [this.data{index}{1}; ...
+                                           t; ...
+                                           zeros(this.sizes{index}-1, 1)];
+                    this.data{index}{2} = [this.data{index}{2}; ...
+                                           x(:).'; ...
+                                           zeros(this.sizes{index}-1, ...
+                                                 length(x(:).'))];
+                    this.sizes{index} = 2 * this.sizes{index};
+                else
+                    this.data{index}{1}(c)    = t;
+                    this.data{index}{2}(c, :) = x(:).';
+                end
+                this.count{index} = c;
+                
+                % Without doubling:
+                % this.data{index}{1} = [this.data{index}{1}; t];
+                % this.data{index}{2} = [this.data{index}{2}; x(:).'];
+                
+                % We should show this data if the user has ever asked to
+                % plot it.
+                this.show{index} = show_it || any(this.show{index});
+                this.group{index} = group;
+                
+            end
+            
+        end
+        
+        function result = contains(this, name)
+        % Return true iff the log contains this name.
+            result = any(strcmp(this.names, name));
+        end
+        
+        function varargout = get_log(this, name)
+        % Return a specific log by name.
+        % 
+        % If one output is request, it returns the data. If two are 
+        % requested, it returns the time and the data. Returns empty if
+        % the logs don't exist (use the 'contains' function to test for
+        % this).
+        % 
+        % x = log.get_log('signal1');
+        % [t, x] = log.get_log('signal1');
+        %
+        % Note that |t| will be ns-by-1 and x will be ns-by-nx, where
+        % nx is the number of elements in a single sample.
+            
+            index = find(strcmp(this.names, name));
+            if isempty(index)
+                varargout = cell(1, nargout);
+                return;
+            else
+                t = this.data{index}{1}(1:this.count{index});
+                x = this.data{index}{2}(1:this.count{index}, :);
+                % Without doubling:
+                % t = this.data{index}{1};
+                % x = this.data{index}{2};
+            end
+            if nargout == 1
+                varargout = {x};
+            elseif nargout == 2
+                varargout = {t, x};
+            end
+        end
+        
+        function plot(this, x_label)
+        % Plot all of the signals, grouped appropriately into figures.
+        %
+        % A custom x label can be added to figures as well (second input).
+            
+            % Get the unique groups.
+            groups = unique(this.group);
+            
+            % For each group...
+            for g = 1:length(groups)
+
+                % See what is to be shown and count them.
+                to_show = find(  strcmp(this.group, groups{g}) ...
+                               & [this.show{:}]);
+                n = length(to_show);
+
+                % Bail if there's no need to show anything.
+                if n == 0, continue; end;
+
+                % Set up the figure.
+                unique_figure(sprintf('LoggerDefaultPlot%d', g), ...
+                              'Name', [groups{g} ' Log']);
+                clf();
+                for k = 1:n
+                    subplot(n, 1, k);
+                    plot(this.data{to_show(k)}{1}(1:this.count{to_show(k)}), ...
+                         this.data{to_show(k)}{2}(1:this.count{to_show(k)}, :));
+                    % plot(this.data{to_show(k)}{1}, ...
+                    %      this.data{to_show(k)}{2});
+                    ylabel(this.names{to_show(k)});
+                end
+                if nargin >= 2, xlabel(x_label); end;
+            
+            end
+                
+        end
+        
+    end
+    
+end
+
+function h = unique_figure(id, varargin)
+% h = unique_figure(id, varargin)
+% 
+% Creates a figure with the given ID (tag) or selects it if it already
+% exists, allowing one to easily reuse the same figure window identified
+% with text instead of handles. This is useful when, e.g., running a script
+% many times after clearing between runs without having to hard-code figure
+% numbers (which can become hard to keep track of).
+% 
+% Any additional arguments are passed along to the figure's |set| method.
+%
+% Example:
+% 
+%   h = unique_figure('trajectory');
+%
+% Example with extra arguments:
+% 
+%   h = unique_figure('trajectory', 'Name', 'Trajectory');
+
+    % See if there's a figure with this ID.
+    h = findobj('Type', 'figure', 'Tag', id);
+    
+    % If not, make it, passing along any options for the figure.
+    if isempty(h)
+        
+        h = figure('Tag', id, varargin{:});
+        
+    else
+        
+        % If we found it, select it.
+        figure(h);
+        
+        % If there were any additional arguments, pass them along. Note
+        % that if there weren't and we called set(h), then it would print h
+        % to the command window -- probably not what we want, hence the
+        % condition.
+        if nargin > 1
+            set(h, varargin{:});
+        end
+        
+    end
+
+end
diff --git a/Lib/various/odehybrid/example_odehybrid_eventfcn.m b/Lib/various/odehybrid/example_odehybrid_eventfcn.m
new file mode 100644
index 0000000..687c2e6
--- /dev/null
+++ b/Lib/various/odehybrid/example_odehybrid_eventfcn.m
@@ -0,0 +1,6 @@
+% A simple example Event function used with examples_odehybrid.
+function [h, t, d] = example_odehybrid_eventfcn(t, x, p, v)
+    h = x(2); % An "event" occurs when the velocity is 0.
+    t = true; % Terminate on event; stop the simulation when h=0.
+    d = 1;    % Trigger when going positive from negative
+end
diff --git a/Lib/various/odehybrid/example_odehybrid_logging.m b/Lib/various/odehybrid/example_odehybrid_logging.m
new file mode 100644
index 0000000..f1430a8
--- /dev/null
+++ b/Lib/various/odehybrid/example_odehybrid_logging.m
@@ -0,0 +1,51 @@
+% A simple example of using logging with odehybrid.
+function example_odehybrid_logging()
+
+    dt  = 0.1;                    % Discrete eq. time step
+    ts  = [0 5];                  % Simulation time
+    x0  = struct('p', 1, 'v', 0); % Initial continuous states
+    d0  = struct('u', 0, 'i', 0); % Initial discrete states
+    log = TimeSeriesLogger();     % Create the logger.
+    
+    % Simulate.
+    [t, sig, td, ctrl] = odehybrid(@ode45, @ode, @de, dt, ts, x0, d0, ...
+                                   [], log);
+    
+    % Plot.
+    plot(t, [sig.p], t, [sig.v], td, [ctrl.u], '.', td, [ctrl.i], '.');
+    xlabel('Time');
+    legend('p', 'v', 'u', '\int p(t)/2 dt');
+    
+    % Add log output.
+    log.plot();
+    xlabel('Time');
+    
+end
+
+% Continuous differential equation
+function dsdt = ode(t, signal, controller, log)
+
+    % Calculate the derivatives.
+    dsdt.p = signal.v;
+    dsdt.v = 2 * signal.p + controller.u + 1;
+    
+    % Log the acceleration. We *must* check to see if the log is passed in;
+    % it won't always be passed in.
+    if nargin >= 4
+        log.add('acceleration', t, dsdt.v);
+    end
+    
+end
+
+% Discrete update equation
+function [signal, controller] = de(t, signal, controller, log)
+
+    % Update the discrete state.
+    controller.u = -8 * signal.p - 4*signal.v - controller.i;
+    controller.i = controller.i + 0.5 * signal.p;
+    
+    % Log the velocity as it was sampled. Logs are always passed to the
+    % discrete update functions, so we don't explicitly need to check.
+    log.add('sampled v', t, signal.v);
+    
+end
diff --git a/Lib/various/odehybrid/example_odehybrid_outputfcn.m b/Lib/various/odehybrid/example_odehybrid_outputfcn.m
new file mode 100644
index 0000000..ee39e93
--- /dev/null
+++ b/Lib/various/odehybrid/example_odehybrid_outputfcn.m
@@ -0,0 +1,23 @@
+% A simple example OutputFcn used with examples_odehybrid.
+function status = example_odehybrid_outputfcn(t, x, p, v, flag)
+
+    % Return true to terminate the propagation.
+    status = 0;
+    
+    switch flag
+        
+        % On init, we receive the time span and init. states.
+        case 'init'
+            fprintf('Simulating from %fs to %fs.\n', t);
+            
+        % When done, times and states are all empty.
+        case 'done'
+            fprintf('Finished the simulation.\n');
+            
+        % Otherwise, we receive 1 or more samples.
+        otherwise
+            status = x(1) < 0; % End the simulation with x(1) < 0.
+            
+    end
+    
+end
diff --git a/Lib/various/odehybrid/example_odehybrid_structs.m b/Lib/various/odehybrid/example_odehybrid_structs.m
new file mode 100644
index 0000000..9a8d4dc
--- /dev/null
+++ b/Lib/various/odehybrid/example_odehybrid_structs.m
@@ -0,0 +1,29 @@
+% A simple example of using structs with odehybrid.
+function example_odehybrid_structs()
+
+    dt = 0.1;                    % Discrete eq. time step
+    ts = [0 5];                  % Simulation time
+    x0 = struct('p', 1, 'v', 0); % Initial continuous states
+    d0 = struct('u', 0, 'i', 0); % Initial discrete states
+    
+    % Simulate.
+    [t, sig, td, ctrl] = odehybrid(@ode45, @ode, @de, dt, ts, x0, d0);
+    
+    % Plot.
+    plot(t, [sig.p], t, [sig.v], td, [ctrl.u], '.', td, [ctrl.i], '.');
+    xlabel('Time');
+    legend('p', 'v', 'u', '\int p(t)/2 dt');
+    
+end
+
+% Continuous differential equation
+function dsdt = ode(t, signal, controller) %#ok<INUSL>
+    dsdt.p = signal.v;
+    dsdt.v = 2 * signal.p + controller.u + 1;
+end
+
+% Discrete update equation
+function [signal, controller] = de(t, signal, controller) %#ok<INUSL>
+    controller.u = -8 * signal.p - 4*signal.v - controller.i;
+    controller.i = controller.i + 0.5 * signal.p;
+end
diff --git a/Lib/various/odehybrid/examples_odehybrid.m b/Lib/various/odehybrid/examples_odehybrid.m
new file mode 100644
index 0000000..a84ad2a
--- /dev/null
+++ b/Lib/various/odehybrid/examples_odehybrid.m
@@ -0,0 +1,555 @@
+%% Examples of |odehybrid|
+%
+% This script contains numerous examples of using |odehybrid| to simulate
+% continuous and discrete systems, from a simple example to more complex 
+% examples with non-vector states and logging, introducing all primary 
+% features of |odehybrid|. It's meant for viewing directly in MATLAB by 
+% clicking "Publish" in the editor or entering:
+%
+%   home = fileparts(which('examples_odehybrid'));
+%   web(fullfile(home, 'html', 'examples_odehybrid.html'));
+% 
+% at the command line. This will run the code and open the result as HTML.
+% 
+% For additional discussion, see:
+% 
+% * Online documentation (<http://www.anuncommonlab.com/doc/odehybrid/>)
+% * Discussion of how simulations work (<http://www.anuncommonlab.com/articles/how-simulations-work/>)
+% * Motivations behind the tool (<http://www.anuncommonlab.com/blog/a-simulation-engine/>)
+%
+% Copyright 2014 An Uncommon Lab
+
+%%
+% <html><center><h1>Basics</h1></center></html>
+
+%% Short and Direct
+% Let's say we have a continuous system in the form:
+%
+% $$\dot{x}_c(t) = f_c(t, x_c(t), x_d(t))$$
+%
+% where $x_c$ is the continuous state and $x_d$ is some state updated at a 
+% discrete time step as
+%
+% $$x_d(t+\Delta t) = f_d(t, x_c(t), x_d(t))$$
+% 
+% Here, we'll give a quick embodiment of this system to show how we can
+% simulate it from its initial condition over time. Afterwards, we'll go
+% more into how it works.
+% 
+% Let |ode|, |de|, |dt|, |x|, and |u| be $f_c$, $f_d$, $\Delta t$, $x_c$, 
+% and $x_d$, respectively.
+
+ode = @(t, x, u) [0 1; 2 0] * x + [0; 1] * u;  % Differential equation
+de  = @(t, x, u) deal(x, -[8 4] * x);          % Discrete update equation
+dt  = 0.1;                                     % Discrete eq. time step
+ts  = [0 5];                                   % From 0 to 5s
+x0  = [1; 0];                                  % Initial continuous state
+u0  = 0;                                       % Initial discrete state
+[t, x, tu, u] = odehybrid(@ode45, ode, de, dt, ts, x0, u0); % Simulate!
+plot(t, x, tu, u, '.'); xlabel('Time');                     % Plot 'em.
+legend('x_1', 'x_2', 'u', 'Location', 'se');                % Label 'em.
+
+%%
+% This was really quick. Notice how the discrete system is updated 
+% regularly every 0.1s, while the continuous system has many more
+% intermediate steps.
+% 
+% Let's look at what we did. First, it's clear that $f_c$ represents 
+% (suppressing the dependence on time):
+% 
+% $$
+%     \left[ \begin{array}{c} \dot{x}_1 \\ \dot{x}_2 \end{array} \right]
+%   =   \left[ \begin{array}{c c} 0 & 1 \\ 2 & 0 \end{array} \right]
+%       \left[ \begin{array}{c} x_1 \\ x_2 \end{array} \right]
+%     + \left[ \begin{array}{c} 0 \\ 1 \end{array} \right]
+%       u
+% $$
+%
+% The discrete state is $u$ and can be thought of as a controller of this 
+% unstable system, guiding it towards [0, 0] (without $u$, the system would
+% diverge from [0, 0]). It's updated at 10Hz according to $f_d$ which is 
+% just:
+%
+% $$
+%   u = \left[ \begin{array}{c c} -8 & -4 \end{array} \right] 
+%       \left[ \begin{array}{c} x_1 \\ x_2 \end{array} \right]
+% $$
+%
+% The second input to the |de| function is the |u| state, but the current
+% |u| won't depend on the last |u|, so it's unused.
+%
+% Notice that the discrete update equation actually has _two_ outputs (the
+% |deal| function maps argument 1 to the first output and argument 2 to the
+% second). This is because |odehybrid| allows the discrete update equation
+% to _also_ update the continuous state as well. This is useful for very
+% fast phenomena, like a quick burst of rocket power to change a
+% satellite's orbit. In MATLAB code, we have:
+% 
+%   [x_c, x_d] = de(t, x_c, x_d);
+%
+% Note that, despite that this example is using linear systems, that has
+% nothing to do with odehybrid. The continuous and discrete equations can
+% be anything (nonlinear, stochastic, user inputs, etc.).
+%
+% For the readers with controls backgrounds, this is clearly a discrete 
+% proportional-derivative controller for the unstable linear system.
+% 
+% That's all there is to setting up this basic system, so let's make things
+% more interesting.
+
+%% Multiple States
+% As state vectors become large, they become unwieldy. It's difficult to
+% remember what the 10th state is, or the 2031st state. For this reason,
+% |odehybrid| allows the use of individual states as individual arguments
+% into the ODE and DE functions.
+%
+% Let's reuse the above example, but let's add something to our discrete
+% controller. Instead of a proportional-derivative controllers, let's make
+% it a proportional-integral-derivative control. Now we'll have two
+% discrete states to keep track of: the input, |u|, and the integral term,
+% |i|, that gets updated every step.
+%
+% $$
+%   u = \left[ \begin{array}{c c c} -8 & -4 & -1 \end{array} \right] 
+%       \left[ \begin{array}{c} x_1 \\ x_2 \\ i \end{array} \right]
+% $$
+%
+% That |i| term is updated as:
+%
+% $$
+%   i = i + \frac{1}{2}x_1
+% $$
+%
+% To use multiple states, we'll need them to be inputs to our |ode| and
+% |de| functions. Plus, the |de| function will have to output the new
+% discrete state.
+%
+% We'll also add a disturbance to the model so the integral term has
+% something to account for. We'll just tack |+ [0; 1]| onto the end.
+%
+% Here's the new |ode| (the final input is the integral term, i, but it 
+% doesn't need it): 
+
+ode = @(t, x, u, i)   [0 1; 2 0] * x ...        % Continuous system
+                    + [0; 1] * u ...            %   with feedback control
+                    + [0; 1];                   %   and with a disturbance
+                
+%%
+% The new |de| is easy too (it uses the integral term, but not the last
+% |u|):
+
+de  = @(t, x, u, i) deal(x, ...                 % No change to cont. state
+                         -[8 4 1] * [x; i], ... % Update input
+                         i + 0.5 * x(1));       % Update integrator
+
+%%
+% The options are the same:
+                     
+dt  = 0.1;                                      % Discrete eq. time step
+ts  = [0 5];                                    % From 0 to 5s
+x0  = [1; 0];                                   % Initial continuous state
+
+%%
+% Two say we have two separate discrete states, we use a cell array for the
+% discrete state and put each initial value in the appropriate place.
+d0  = {0, 0};                                   % Initial discrete states
+
+%%
+% The discrete states are output separately:
+
+[t, x, td, u, i] = odehybrid(@ode45, ode, de, dt, ts, x0, d0); % Simulate!
+plot(t, x, td, [u, i], '.'); xlabel('Time');                   % Plot 'em.
+legend('x_1', 'x_2', 'u', '\int x_1(t)/2 dt');                 % Label 'em.
+
+%%
+% That's still pretty easy to implement. We can expand the continuous
+% states too. The important part is that the |ode| has the form:
+%
+%   [x_dot_1, x_dot_2, ..] = ode(t, x1, x2, .., xd1, xd2, ..);
+%
+% And the |de| has the form:
+% 
+%   [x1, x2, .., xd1, xd2, ..] = de(t, x1, x2, .., xd1, xd2, ..);
+%
+% After simulation is done, |odehybrid| will output:
+%
+%   [t, x1, x2, .., td, xd1, xd2, ..] = odehybrid(@ode45, ...
+%                                                 ode, de, dt, ts, ...
+%                                                 {x1, x2, ...}, ...
+%                                                 {xd1, xd2, ...});
+%                                                 
+
+%% Multiple Continuous and Discrete States
+% We'll quickly re-implement the above, with multiple continuous and
+% discrete states. Let's use |p| for $x_1$ and |v| for $x_2$.
+
+ode = @(t, p, v, u, i) deal(v, ...              % dp/dt (same as before)
+                            2 * p + u + 1);     % dv/dt (same as before)
+de  = @(t, p, v, u, i) deal(p, ...              % No change to p
+                            v, ...              % No change to v
+                            -8*p - 4*v - i, ... % Update input
+                            i + 0.5 * p);       % Update integrator
+dt  = 0.1;                                      % Discrete eq. time step
+ts  = [0 5];                                    % From 0 to 5s
+x0  = {1; 0};                                   % Initial continuous states
+d0  = {0, 0};                                   % Initial discrete states
+[t, p, v, td, u, i] = odehybrid(@ode45, ode, de, dt, ts, x0, d0);
+plot(t, [p, v], td, [u, i], '.'); xlabel('Time');
+legend('p', 'v', 'u', '\int p(t)/2 dt');
+
+num_steps = length(t);
+
+%%
+% Note how all of states are inputs to |ode| and |de| (and therefore must
+% be outputs). Also notice how all of the states are output from
+% |odehybrid|.
+
+%% Using Structs for States
+% Aside from using multiple states, we can also use states that aren't
+% simple scalars or vectors. For instance, we can use structs. As a
+% system begins to have many subsystems, using structs is a good way to
+% keep track of scope for all of the states. For instance, we might have
+% |car_sw.cruise_ctrl.speed| and |car_sw.monitor.lane_watcher|, clearly
+% keeping variables scoped to specific systems. Plus, it's easy to hand off
+% relevant substructs to the subsystems:
+%
+%   car_sw.cruise_ctrl = cruise_alg(car_sw.cruise_ctrl, current_speed);
+%   % ...
+%   car_sw.monitor = monitor_alg(car_sw.monitor, sensor_bus, ..);
+
+%%
+% Let's continue our PID controller example, making the discrete state a
+% structure with fields for |u| and |i|, and let's also make the continuous
+% state a struct with fields for |p| and |v|.
+
+type example_odehybrid_structs
+
+%%
+% Updating the structures with the discrete update equation is clear
+% (simply update the structs and return them). For the continuous state,
+% we return structs with the same structure, but the values they contain
+% are the time derivatives. Notice in the above out the output struct of
+% the differential equation is called |dsdt| for "d-signal-by-dt". Its |p|
+% field is really $dp/dt$ and similarly its |v| field is really $dv/dt$. In
+% this way, all of the values in the input struct can be correctly updated 
+% with the corresponding derivatives.
+
+%%
+example_odehybrid_structs();
+
+%% Using Other Types
+% We can mix and match states of different types, including numeric types,
+% structs, and cell arrays. Here's an example set of discrete states with
+% all three:
+%
+%   d0 = {magic(3),                  % 2D array
+%         struct('mode', 1),         % struct
+%         {5, [1 2 3], [1 2; 3 4]}}; % cell array
+%
+% Numeric types can also have any data type, e.g. int16, and this will be
+% handled appropriately.
+%
+% There is one important thing to note: the size and structure of these
+% states can't change during the simulation. For instance, if one state
+% were a string for the status, e.g., 'online', and the status were changed
+% in the simulation to 'offline' (a different number of characters), there
+% would be an error. Instead, one can either use numbers to represent
+% online or offline or one could pad online with an extra space, i.e.,
+% 'online '.
+%
+% For advanced types: if custom objects are desired for the states, those 
+% can be used to. However, they must have a |state_to_vector| method that 
+% returns a complete representation of the internal state numerically and a
+% |vector_to_state| method that reconstructs the object from a vector. See
+% those two function respectively.
+
+%% Using Logging
+% Good simulations allow a lot of analysis. So far, we've only been able to
+% get states out of the system and to analyze those states, but what if
+% there were some intermediate variable we wanted to display? We could use
+% MATLAB's debugger to observe their values at each sample, but it's often
+% better to store all of those values and examine them afterwards --
+% logging. These tools include a logging mechanism called TimeSeriesLogger 
+% (it has nothing to do with the |timeseries| class in MATLAB). It's pretty
+% easy to use. Let's take a look.
+
+log = TimeSeriesLogger(); % Create it.
+for t = 0:100
+    log.add('signal 1', t, randn());
+    log.add('signal 2', t, 10*rand());
+end
+log.plot();
+
+%%
+% There are many additional options (see the |help TimeSeriesLogger|), but
+% let's focus on how to use this with our simulation. We'll continue to
+% modify the struct example above. This will consist basically of:
+%
+% # Creating a TimeSeriesLogger and passing it to |odehybrid|.
+% # Adding a |log| input to the |ode| and |de| functions.
+% # Adding some logging inside those functions.
+% # Plotting the results when the simulation is done.
+%
+% Here are the new bits:
+% 
+%   log = TimeSeriesLogger();     % Create the logger.
+%   % ...
+%   [t, sig, td, ctrl] = odehybrid(@ode45, @ode, @de, dt, ts, x0, d0, ...
+%                                  [], log);
+%   % ...
+% 
+%   function dsdt = ode(t, signal, controller, log) ...
+%       % ...
+%       if nargin >= 4
+%           log.add('acceleration', t, dsdt.v);
+%       end
+%       % ...
+% 
+%   function [signal, controller] = de(t, signal, controller, log)
+%       %...
+%       log.add('sampled v', t, signal.v);
+%       % ...
+
+%%
+% Here's the complete simulator:
+type example_odehybrid_logging
+
+%%
+% And here's the output from the log.
+example_odehybrid_logging();
+
+%%
+% There we have it. Note that the log _won't_ always be passed in to the
+% ODE. The reason for this is that sometimes the continuous propagators
+% take a large time step, see that the step was too large, and go back and
+% take a smaller time step. The first step is discarded. Because we don't
+% want phantom data points from discarded time steps, |odehybrid| doesn't
+% pass in the logger until the steps are finalized.
+
+%%
+% <html><center><h1>Beyond the Basics</h1></center></html>
+
+%% Interpolation
+% There's a quick detail that must be pointed out about discrete-time
+% updates. Because these update states _directly_, those states change
+% _instantaneously_. That is, at say, t=3 seconds, there's the "original"
+% value of a state ("just to the left" of 3, usually denoted as $3^-$).
+% Then there's the updated value at $3^+$. So our time and state history
+% might look like this:
+
+t = [2.76, 2.91, 3,   3,   3.12].';
+x = [0.2,  0.3,  0.4, 1.1, 1.2].';
+
+clf();
+plot(t, x, '.-');
+
+%%
+% If we were to try to interpolate this data with |interp1|, it wouldn't
+% work, because it wouldn't know how to handle the doubled 3.
+% 
+% Therefore, |odehybrid| includes a function called |interpd| (for
+% "interpolate, discrete") which can handle this type of thing. Simply
+% specific whether the "-" or "+" values are desired during interpolation.
+% For |+|, all interpolation between |t| = 2.91 and 3 will use [0.3 0.4] 
+% while exactly at 3 the datapoint will be 1.7, and interpolation
+% afterwards uses [1.7 and 1.8]. For |-|, it's the same, except the value
+% right at 3 is 0.4.
+
+% Resample |t| and |x| at |ti| using the "right" value:
+ti = (2.8:0.1:3.1).';
+xi = interpd(t, x, ti, '+').'
+xi = interpd(t, x, ti, '-').'
+
+%% Other Propagators
+% While |ode23| and |ode45|, etc., are robust and convenient tools
+% representing some of what's best about MATLAB, |odehybrid| can expose a
+% few pecularities. For instance, regardless of the dynamics, those 
+% functions will always take _at least_ 41 steps to propagate (regardless 
+% of what the 'InitialStep' value is in the |odeset| structure). Since 
+% |odehybrid| uses |ode45| (or whatever propagator is passed in) between 
+% the discrete steps, this means there will be at least 41 time steps 
+% representing the continuous dynamics between the discrete steps. Ouch! 
+% That can be way too much. For this reason, |odehybrid| includes a few of 
+% propagators with the same interface as the |odefun| functions. Perhaps 
+% the easiest is |rkadapt|, which is essentially a drop-in replacement for 
+% |@ode45| in the examples above.
+
+[t, p, v, td, u, i] = odehybrid(@rkadapt, ode, de, dt, ts, x0, d0);
+
+fprintf('This simulation took %.0f%% of the steps required by ode45.\n',...
+        100*length(t)/num_steps);
+
+%%
+% The |ode45| and |rkadapt| functions are _adaptive_ time-step propagators.
+% This means they "figure out" an appropriate time step to take (see the
+% linked article on how simulations work). However, if one knows a good
+% time step, that can be specified with a fixed-step solver. MATLAB doesn't
+% include any of these, so |odehybrid| includes two: |rk4|, the most common
+% fixed-step propagator, and |rkfixed|, which can implement any fixed-step 
+% Runge-Kutta method given the appropriate arguments. We'll show an example
+% using |rk4|. Note that the time step is specified witht he |odeset|
+% structure.
+
+options = odeset('MaxStep', dt);
+[t, p, v, td, u, i] = odehybrid(@rk4, ode, de, dt, ts, x0, d0, options);
+
+fprintf('This simulation took %.0f%% of the steps required by ode45.\n',...
+        100*length(t)/num_steps);
+
+%% Multiple Discrete Updates
+% Using multiple discrete update equations is as passing multiple function
+% (in a cell array) in for |de| and passing their corresponding time steps 
+% in for |dt|. Here is a simple harmonic oscillator. The discrete updates
+% only "sample" the oscillator, with the first update recording the
+% "position" term and the second update recording the "velocity" term. Each
+% discrete update must take in and return all continuous and discrete
+% states (just like for the case with a single discrete update equation).
+
+ode = @(t, x, p, v) [0 1; -1 0] * x;         % Differential equation
+de  = {@(t, x, p, v) deal(x, x(1), v); ...   % Discrete update equation 1
+       @(t, x, p, v) deal(x, p, x(2))};      % Discrete update equation 2
+dt  = [0.1, 0.15];                           % Discrete eq. time steps
+ts  = [0 2*pi];                              % From 0 to 5s
+x0  = [1; 0];                                % Initial continuous state
+y0  = {0, 0};                                % Initial discrete state
+
+[t, x, ty, y1, y2] = odehybrid(@rkfixed, ode, de, dt, ts, x0, y0);
+
+clf();
+plot(t, x, '.-');
+hold on;
+stairs(ty, [y1 y2], ':');
+hold off;
+legend('Position', 'Velocity', 'Position (0.1s)', 'Velocity (0.15s)');
+xlabel('Time (s)');
+
+%%
+% <html><center><h1>Advanced Options</h1></center></html>
+
+%% Using an OutputFcn
+% The |ode45| function allows one to specify an |OutputFcn| to be called
+% when the states are updated. This function can, e.g. update plots or 
+% determine if the simulation should stop (if it returns anything but 0).
+% |odehybrid| allows this to pass through, but with all of the arguments 
+% separated just like for the ODE and DEs. For instance, here's a simple
+% |OutputFcn|:
+
+type example_odehybrid_outputfcn;
+
+%%
+% Let's add this to the simulation above.
+options = odeset('OutputFcn', @example_odehybrid_outputfcn);
+[t, x, ty, y1, y2] = odehybrid(@rkfixed, ode, de, dt, ts, x0, y0, options);
+
+%%
+% Note that the simulation stopped when |x(1)| fell below 0.
+plot(t, x);
+xlabel('Time (s)');
+
+%% Using Events
+% Another useful property of MATLAB's ODE suite is events. These are used
+% to "narrow in" on a specific occurrence. To use event, one write an event
+% function, which should return a continuous value. The propagator will
+% search for the time at which that value crosses 0. For instance, suppose
+% one wanted a simulation to end when a parachutist lands on the ground.
+% Then one could write an event function that returns her height above the
+% ground. The propagator will take small time steps towards the moment when
+% the height achieves 0. The event function should also return two more
+% pieces of information: if the simulation is to stop when the event occurs
+% and the direction of the zero crossing. See |doc odeset| for more on
+% Events.
+%
+% In |odehybrid|, events are like those for |ode45|, but take the full set
+% of states as arguments, just like the ODE, DE, and |OutputFcn|. This
+% function will cause propagation to terminate when the velocity (|x(2)|)
+% goes to zero from below.
+
+type example_odehybrid_eventfcn
+
+%%
+% Let's add this to the simulation above. Note that we can only use events
+% with the ODE suite functions. Also, note all the "event" outputs.
+
+options = odeset('Events', @example_odehybrid_eventfcn);
+[t, x, ty, y1, y2, te, xe, y1e, y2e, ie] = ...
+                       odehybrid(@ode45, ode, de, dt, ts, x0, y0, options);
+plot(t, x, ...
+     te, xe, 'o');
+xlabel('Time (s)');
+
+%%
+% An event function can return vectors instead of scalars, representing
+% multiple possible zero-crossings. The triggering event index is output in
+% |ie|.
+
+%% Using Structs for Output
+% If having eight output arguments is getting to be too many, we can
+% instead use the struct output form.
+
+sol = odehybrid(@ode45, ode, de, dt, ts, x0, y0, options)
+
+%%
+% The struct has a field for each output of |odehybrid|, with the states 
+% grouped into cell arrays.
+
+%%
+% <html><center><h1>Comments</h1></center></html>
+
+%% Notes on Speed
+% Most folks would like their simulations to run fast. Here are a few
+% things to consider.
+% 
+% The first thing, which doesn't really have to do with the tools at all,
+% is to make sure the algorithms make sense. For instance, are you
+% simulating a stiff subsystem in the middle of an otherwise loose
+% system (a part with a very fast reaciton time compared to other parts)?
+% That likely causes the entire simulation to run slowly, and often for the
+% purpose of the larger simulation, the stiffest systems can be replaced 
+% with functional equivalents (like a table lookup).
+%
+% The next most important thing is to make the MATLAB code representing the
+% ODE and the DE as fast as possible. See the MATLAB documentation under
+% "Advaned Software Development > Performance and Memory > Code
+% Performance" (R2014a).
+% 
+% Choosing the right continuous propagator also makes a large difference.
+% While |ode45| is usually the best place to start, there are better 
+% choices are for different types of problems. For MATLAB's ODE 
+% propagators, see the MATLAB documentation under "Mathematics > Numerical 
+% Integration... > Ordinary Differential Equations". If MATLAB's
+% propagators are taking an excessive number of steps for the 
+% continuous-time portions (as above and as is common for control 
+% problems), consider using the included 
+% |<matlab:doc('rk4') rk4>| or |<matlab:doc('rkfixed') rkfixed>| functions.
+% One can also write one's own propagator or pass any Runge-Kutta method's
+% details to |rkfixed| for an instant, custom propagator.
+%
+% Using non-numeric types (like structs or cell arrays) for states is
+% convenient and can dramatically reduce development and debugging time. 
+% However, there is naturally a speed penalty. How big that penalty is
+% depends the complexity of the state and the functions using the state. In
+% the author's simulations, it's often only about 20% of the overall 
+% simulation time, which is a bargain considering the convenience. This
+% should be a last resort.
+% 
+% Finally, logging naturally adds overhead as well. However, some care was
+% taken in minimize the performance impact from logging, and it's often a
+% smaller concern even than using non-numeric types -- less than 20% of the
+% run time. For large numberes of simulations for which detailed analysis
+% won't be necessary, one can simply turn logging off (pass |[]| for the
+% |log| argument).
+
+%%
+% <html><center><h1>Summary</h1></center></html>
+
+%% Summary
+% We've covered a lot of ground, including how to set up continuous and
+% discrete simulations, use complex states, add logging, work with the
+% results, and achieve better speed. At this point, a careful reader should
+% be able to start making simulations with |odehybrid|. However, this is 
+% not the end of the documentation; it's just a jumping off point. For more
+% info, see:
+% 
+% * <http://www.anuncommonlab.com/doc/odehybrid/ The online documentation>
+% * Help for individual functions (e.g., |doc TimeSeriesLogger|)
+% * <http://www.anuncommonlab.com/articles/how-simulations-work How simulations work>
diff --git a/Lib/various/odehybrid/fede_odehybrid.m b/Lib/various/odehybrid/fede_odehybrid.m
new file mode 100644
index 0000000..9269cc4
--- /dev/null
+++ b/Lib/various/odehybrid/fede_odehybrid.m
@@ -0,0 +1,64 @@
+%% Examples of |odehybrid|
+%
+% This script contains numerous examples of using |odehybrid| to simulate
+% continuous and discrete systems, from a simple example to more complex 
+% examples with non-vector states and logging, introducing all primary 
+% features of |odehybrid|. It's meant for viewing directly in MATLAB by 
+% clicking "Publish" in the editor or entering:
+%
+%   home = fileparts(which('examples_odehybrid'));
+%   web(fullfile(home, 'html', 'examples_odehybrid.html'));
+% 
+% at the command line. This will run the code and open the result as HTML.
+% 
+% For additional discussion, see:
+% 
+% * Online documentation (<http://www.anuncommonlab.com/doc/odehybrid/>)
+% * Discussion of how simulations work (<http://www.anuncommonlab.com/articles/how-simulations-work/>)
+% * Motivations behind the tool (<http://www.anuncommonlab.com/blog/a-simulation-engine/>)
+%
+% Copyright 2014 An Uncommon Lab
+
+%%
+% <html><center><h1>Basics</h1></center></html>
+
+%% Short and Direct
+% Let's say we have a continuous system in the form:
+%
+% $$\dot{x}_c(t) = f_c(t, x_c(t), x_d(t))$$
+%
+% where $x_c$ is the continuous state and $x_d$ is some state updated at a 
+% discrete time step as
+%
+% $$x_d(t+\Delta t) = f_d(t, x_c(t), x_d(t))$$
+% 
+% Here, we'll give a quick embodiment of this system to show how we can
+% simulate it from its initial condition over time. Afterwards, we'll go
+% more into how it works.
+% 
+% Let |ode|, |de|, |dt|, |x|, and |u| be $f_c$, $f_d$, $\Delta t$, $x_c$, 
+% and $x_d$, respectively.
+
+% define params
+params.decay = 0.01;
+params.theta = 0.5;
+
+dt  = 0.1;                                     % Discrete eq. time step
+ts  = [0 5];                                   % From 0 to 5s
+xf0  = 1;                                      % Initial continuous state
+xj0  = 0;                                      % Initial discrete state
+[tf, xf, tj, xj] = odehybrid(@ode45, @(x,t,parans)flow_map(x,t,params), @(x,t,params)jump_map, dt, ts, xf0, xj0,params); % Simulate!
+plot(tf, xf, tj, xj, '.'); xlabel('Time');                     % Plot 'em.
+legend('x_f', 'x_j', 'Location', 'se');                % Label 'em.
+
+%%% the functions will be a little more involved
+function x_dot = flow_map(x,t,params)
+    x_dot(1) = -params.decay*x(1);
+end
+
+function x_plus = jump_map(x,t,params)
+    x_plus(1) = params.theta*x(1) + (1-params.theta)*rand;
+end
+
+
+
diff --git a/Lib/various/odehybrid/html/Thumbs.db b/Lib/various/odehybrid/html/Thumbs.db
new file mode 100644
index 0000000..aabcdd3
Binary files /dev/null and b/Lib/various/odehybrid/html/Thumbs.db differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid.html b/Lib/various/odehybrid/html/examples_odehybrid.html
new file mode 100644
index 0000000..077bb3a
--- /dev/null
+++ b/Lib/various/odehybrid/html/examples_odehybrid.html
@@ -0,0 +1,883 @@
+
+<!DOCTYPE html
+  PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
+<html><head>
+      <meta http-equiv="Content-Type" content="text/html; charset=utf-8">
+   <!--
+This HTML was auto-generated from MATLAB code.
+To make changes, update the MATLAB code and republish this document.
+      --><title>Examples of odehybrid</title><meta name="generator" content="MATLAB 8.5"><link rel="schema.DC" href="http://purl.org/dc/elements/1.1/"><meta name="DC.date" content="2015-05-23"><meta name="DC.source" content="examples_odehybrid.m"><style type="text/css">
+html,body,div,span,applet,object,iframe,h1,h2,h3,h4,h5,h6,p,blockquote,pre,a,abbr,acronym,address,big,cite,code,del,dfn,em,font,img,ins,kbd,q,s,samp,small,strike,strong,sub,sup,tt,var,b,u,i,center,dl,dt,dd,ol,ul,li,fieldset,form,label,legend,table,caption,tbody,tfoot,thead,tr,th,td{margin:0;padding:0;border:0;outline:0;font-size:100%;vertical-align:baseline;background:transparent}body{line-height:1}ol,ul{list-style:none}blockquote,q{quotes:none}blockquote:before,blockquote:after,q:before,q:after{content:'';content:none}:focus{outine:0}ins{text-decoration:none}del{text-decoration:line-through}table{border-collapse:collapse;border-spacing:0}
+
+html { min-height:100%; margin-bottom:1px; }
+html body { height:100%; margin:0px; font-family:Arial, Helvetica, sans-serif; font-size:10px; color:#000; line-height:140%; background:#fff none; overflow-y:scroll; }
+html body td { vertical-align:top; text-align:left; }
+
+h1 { padding:0px; margin:0px 0px 25px; font-family:Arial, Helvetica, sans-serif; font-size:1.5em; color:#d55000; line-height:100%; font-weight:normal; }
+h2 { padding:0px; margin:0px 0px 8px; font-family:Arial, Helvetica, sans-serif; font-size:1.2em; color:#000; font-weight:bold; line-height:140%; border-bottom:1px solid #d6d4d4; display:block; }
+h3 { padding:0px; margin:0px 0px 5px; font-family:Arial, Helvetica, sans-serif; font-size:1.1em; color:#000; font-weight:bold; line-height:140%; }
+
+a { color:#005fce; text-decoration:none; }
+a:hover { color:#005fce; text-decoration:underline; }
+a:visited { color:#004aa0; text-decoration:none; }
+
+p { padding:0px; margin:0px 0px 20px; }
+img { padding:0px; margin:0px 0px 20px; border:none; }
+p img, pre img, tt img, li img, h1 img, h2 img { margin-bottom:0px; } 
+
+ul { padding:0px; margin:0px 0px 20px 23px; list-style:square; }
+ul li { padding:0px; margin:0px 0px 7px 0px; }
+ul li ul { padding:5px 0px 0px; margin:0px 0px 7px 23px; }
+ul li ol li { list-style:decimal; }
+ol { padding:0px; margin:0px 0px 20px 0px; list-style:decimal; }
+ol li { padding:0px; margin:0px 0px 7px 23px; list-style-type:decimal; }
+ol li ol { padding:5px 0px 0px; margin:0px 0px 7px 0px; }
+ol li ol li { list-style-type:lower-alpha; }
+ol li ul { padding-top:7px; }
+ol li ul li { list-style:square; }
+
+.content { font-size:1.2em; line-height:140%; padding: 20px; }
+
+pre, code { font-size:12px; }
+tt { font-size: 1.2em; }
+pre { margin:0px 0px 20px; }
+pre.codeinput { padding:10px; border:1px solid #d3d3d3; background:#f7f7f7; }
+pre.codeoutput { padding:10px 11px; margin:0px 0px 20px; color:#4c4c4c; }
+pre.error { color:red; }
+
+@media print { pre.codeinput, pre.codeoutput { word-wrap:break-word; width:100%; } }
+
+span.keyword { color:#0000FF }
+span.comment { color:#228B22 }
+span.string { color:#A020F0 }
+span.untermstring { color:#B20000 }
+span.syscmd { color:#B28C00 }
+
+.footer { width:auto; padding:10px 0px; margin:25px 0px 0px; border-top:1px dotted #878787; font-size:0.8em; line-height:140%; font-style:italic; color:#878787; text-align:left; float:none; }
+.footer p { margin:0px; }
+.footer a { color:#878787; }
+.footer a:hover { color:#878787; text-decoration:underline; }
+.footer a:visited { color:#878787; }
+
+table th { padding:7px 5px; text-align:left; vertical-align:middle; border: 1px solid #d6d4d4; font-weight:bold; }
+table td { padding:7px 5px; text-align:left; vertical-align:top; border:1px solid #d6d4d4; }
+
+
+
+
+
+  </style></head><body><div class="content"><h1>Examples of <tt>odehybrid</tt></h1><!--introduction--><p>This script contains numerous examples of using <tt>odehybrid</tt> to simulate continuous and discrete systems, from a simple example to more complex examples with non-vector states and logging, introducing all primary features of <tt>odehybrid</tt>. It's meant for viewing directly in MATLAB by clicking "Publish" in the editor or entering:</p><pre class="language-matlab">home = fileparts(which(<span class="string">'examples_odehybrid'</span>));
+web(fullfile(home, <span class="string">'html'</span>, <span class="string">'examples_odehybrid.html'</span>));
+</pre><p>at the command line. This will run the code and open the result as HTML.</p><p>For additional discussion, see:</p><div><ul><li>Online documentation (<a href="http://www.anuncommonlab.com/doc/odehybrid/">http://www.anuncommonlab.com/doc/odehybrid/</a>)</li><li>Discussion of how simulations work (<a href="http://www.anuncommonlab.com/articles/how-simulations-work/">http://www.anuncommonlab.com/articles/how-simulations-work/</a>)</li><li>Motivations behind the tool (<a href="http://www.anuncommonlab.com/blog/a-simulation-engine/">http://www.anuncommonlab.com/blog/a-simulation-engine/</a>)</li></ul></div><p>Copyright 2014 An Uncommon Lab</p><!--/introduction--><h2>Contents</h2><div><ul><li><a href="#2">Short and Direct</a></li><li><a href="#4">Multiple States</a></li><li><a href="#10">Multiple Continuous and Discrete States</a></li><li><a href="#12">Using Structs for States</a></li><li><a href="#16">Using Other Types</a></li><li><a href="#17">Using Logging</a></li><li><a href="#23">Interpolation</a></li><li><a href="#25">Other Propagators</a></li><li><a href="#27">Multiple Discrete Updates</a></li><li><a href="#29">Using an OutputFcn</a></li><li><a href="#32">Using Events</a></li><li><a href="#35">Using Structs for Output</a></li><li><a href="#38">Notes on Speed</a></li><li><a href="#40">Summary</a></li></ul></div><p><center><h1>Basics</h1></center></p><h2>Short and Direct<a name="2"></a></h2><p>Let's say we have a continuous system in the form:</p><p><img src="examples_odehybrid_eq11099118058511462397.png" alt="$$\dot{x}_c(t) = f_c(t, x_c(t), x_d(t))$$"></p><p>where <img src="examples_odehybrid_eq03870024100092193585.png" alt="$x_c$"> is the continuous state and <img src="examples_odehybrid_eq07737725388692818491.png" alt="$x_d$"> is some state updated at a discrete time step as</p><p><img src="examples_odehybrid_eq08537354732563299936.png" alt="$$x_d(t+\Delta t) = f_d(t, x_c(t), x_d(t))$$"></p><p>Here, we'll give a quick embodiment of this system to show how we can simulate it from its initial condition over time. Afterwards, we'll go more into how it works.</p><p>Let <tt>ode</tt>, <tt>de</tt>, <tt>dt</tt>, <tt>x</tt>, and <tt>u</tt> be <img src="examples_odehybrid_eq06902086386518730424.png" alt="$f_c$">, <img src="examples_odehybrid_eq07323985336366517770.png" alt="$f_d$">, <img src="examples_odehybrid_eq07651498126111630170.png" alt="$\Delta t$">, <img src="examples_odehybrid_eq03870024100092193585.png" alt="$x_c$">, and <img src="examples_odehybrid_eq07737725388692818491.png" alt="$x_d$">, respectively.</p><pre class="codeinput">ode = @(t, x, u) [0 1; 2 0] * x + [0; 1] * u;  <span class="comment">% Differential equation</span>
+de  = @(t, x, u) deal(x, -[8 4] * x);          <span class="comment">% Discrete update equation</span>
+dt  = 0.1;                                     <span class="comment">% Discrete eq. time step</span>
+ts  = [0 5];                                   <span class="comment">% From 0 to 5s</span>
+x0  = [1; 0];                                  <span class="comment">% Initial continuous state</span>
+u0  = 0;                                       <span class="comment">% Initial discrete state</span>
+[t, x, tu, u] = odehybrid(@ode45, ode, de, dt, ts, x0, u0); <span class="comment">% Simulate!</span>
+plot(t, x, tu, u, <span class="string">'.'</span>); xlabel(<span class="string">'Time'</span>);                     <span class="comment">% Plot 'em.</span>
+legend(<span class="string">'x_1'</span>, <span class="string">'x_2'</span>, <span class="string">'u'</span>, <span class="string">'Location'</span>, <span class="string">'se'</span>);                <span class="comment">% Label 'em.</span>
+</pre><img vspace="5" hspace="5" src="examples_odehybrid_01.png" alt=""> <p>This was really quick. Notice how the discrete system is updated regularly every 0.1s, while the continuous system has many more intermediate steps.</p><p>Let's look at what we did. First, it's clear that <img src="examples_odehybrid_eq06902086386518730424.png" alt="$f_c$"> represents (suppressing the dependence on time):</p><p><img src="examples_odehybrid_eq14964292944995088614.png" alt="$$&#xA;    \left[ \begin{array}{c} \dot{x}_1 \\ \dot{x}_2 \end{array} \right]&#xA;  =   \left[ \begin{array}{c c} 0 &amp; 1 \\ 2 &amp; 0 \end{array} \right]&#xA;      \left[ \begin{array}{c} x_1 \\ x_2 \end{array} \right]&#xA;    + \left[ \begin{array}{c} 0 \\ 1 \end{array} \right]&#xA;      u&#xA;$$"></p><p>The discrete state is <img src="examples_odehybrid_eq11776305044305525613.png" alt="$u$"> and can be thought of as a controller of this unstable system, guiding it towards [0, 0] (without <img src="examples_odehybrid_eq11776305044305525613.png" alt="$u$">, the system would diverge from [0, 0]). It's updated at 10Hz according to <img src="examples_odehybrid_eq07323985336366517770.png" alt="$f_d$"> which is just:</p><p><img src="examples_odehybrid_eq04503507910695345665.png" alt="$$&#xA;  u = \left[ \begin{array}{c c} -8 &amp; -4 \end{array} \right]&#xA;      \left[ \begin{array}{c} x_1 \\ x_2 \end{array} \right]&#xA;$$"></p><p>The second input to the <tt>de</tt> function is the <tt>u</tt> state, but the current <tt>u</tt> won't depend on the last <tt>u</tt>, so it's unused.</p><p>Notice that the discrete update equation actually has <i>two</i> outputs (the <tt>deal</tt> function maps argument 1 to the first output and argument 2 to the second). This is because <tt>odehybrid</tt> allows the discrete update equation to <i>also</i> update the continuous state as well. This is useful for very fast phenomena, like a quick burst of rocket power to change a satellite's orbit. In MATLAB code, we have:</p><pre class="language-matlab">[x_c, x_d] = de(t, x_c, x_d);
+</pre><p>Note that, despite that this example is using linear systems, that has nothing to do with odehybrid. The continuous and discrete equations can be anything (nonlinear, stochastic, user inputs, etc.).</p><p>For the readers with controls backgrounds, this is clearly a discrete proportional-derivative controller for the unstable linear system.</p><p>That's all there is to setting up this basic system, so let's make things more interesting.</p><h2>Multiple States<a name="4"></a></h2><p>As state vectors become large, they become unwieldy. It's difficult to remember what the 10th state is, or the 2031st state. For this reason, <tt>odehybrid</tt> allows the use of individual states as individual arguments into the ODE and DE functions.</p><p>Let's reuse the above example, but let's add something to our discrete controller. Instead of a proportional-derivative controllers, let's make it a proportional-integral-derivative control. Now we'll have two discrete states to keep track of: the input, <tt>u</tt>, and the integral term, <tt>i</tt>, that gets updated every step.</p><p><img src="examples_odehybrid_eq17804952888730603842.png" alt="$$&#xA;  u = \left[ \begin{array}{c c c} -8 &amp; -4 &amp; -1 \end{array} \right]&#xA;      \left[ \begin{array}{c} x_1 \\ x_2 \\ i \end{array} \right]&#xA;$$"></p><p>That <tt>i</tt> term is updated as:</p><p><img src="examples_odehybrid_eq11149857664895560545.png" alt="$$&#xA;  i = i + \frac{1}{2}x_1&#xA;$$"></p><p>To use multiple states, we'll need them to be inputs to our <tt>ode</tt> and <tt>de</tt> functions. Plus, the <tt>de</tt> function will have to output the new discrete state.</p><p>We'll also add a disturbance to the model so the integral term has something to account for. We'll just tack <tt>+ [0; 1]</tt> onto the end.</p><p>Here's the new <tt>ode</tt> (the final input is the integral term, i, but it doesn't need it):</p><pre class="codeinput">ode = @(t, x, u, i)   [0 1; 2 0] * x <span class="keyword">...</span><span class="comment">        % Continuous system</span>
+                    + [0; 1] * u <span class="keyword">...</span><span class="comment">            %   with feedback control</span>
+                    + [0; 1];                   <span class="comment">%   and with a disturbance</span>
+</pre><p>The new <tt>de</tt> is easy too (it uses the integral term, but not the last <tt>u</tt>):</p><pre class="codeinput">de  = @(t, x, u, i) deal(x, <span class="keyword">...</span><span class="comment">                 % No change to cont. state</span>
+                         -[8 4 1] * [x; i], <span class="keyword">...</span><span class="comment"> % Update input</span>
+                         i + 0.5 * x(1));       <span class="comment">% Update integrator</span>
+</pre><p>The options are the same:</p><pre class="codeinput">dt  = 0.1;                                      <span class="comment">% Discrete eq. time step</span>
+ts  = [0 5];                                    <span class="comment">% From 0 to 5s</span>
+x0  = [1; 0];                                   <span class="comment">% Initial continuous state</span>
+</pre><p>Two say we have two separate discrete states, we use a cell array for the discrete state and put each initial value in the appropriate place.</p><pre class="codeinput">d0  = {0, 0};                                   <span class="comment">% Initial discrete states</span>
+</pre><p>The discrete states are output separately:</p><pre class="codeinput">[t, x, td, u, i] = odehybrid(@ode45, ode, de, dt, ts, x0, d0); <span class="comment">% Simulate!</span>
+plot(t, x, td, [u, i], <span class="string">'.'</span>); xlabel(<span class="string">'Time'</span>);                   <span class="comment">% Plot 'em.</span>
+legend(<span class="string">'x_1'</span>, <span class="string">'x_2'</span>, <span class="string">'u'</span>, <span class="string">'\int x_1(t)/2 dt'</span>);                 <span class="comment">% Label 'em.</span>
+</pre><img vspace="5" hspace="5" src="examples_odehybrid_02.png" alt=""> <p>That's still pretty easy to implement. We can expand the continuous states too. The important part is that the <tt>ode</tt> has the form:</p><pre class="language-matlab">[x_dot_1, x_dot_2, ..] = ode(t, x1, x2, .., xd1, xd2, ..);
+</pre><p>And the <tt>de</tt> has the form:</p><pre class="language-matlab">[x1, x2, .., xd1, xd2, ..] = de(t, x1, x2, .., xd1, xd2, ..);
+</pre><p>After simulation is done, <tt>odehybrid</tt> will output:</p><pre class="language-matlab">[t, x1, x2, .., td, xd1, xd2, ..] = odehybrid(@ode45, <span class="keyword">...</span>
+                                              ode, de, dt, ts, <span class="keyword">...</span>
+                                              {x1, x2, <span class="keyword">...</span><span class="comment">}, ...</span>
+                                              {xd1, xd2, <span class="keyword">...</span><span class="comment">});</span>
+</pre><h2>Multiple Continuous and Discrete States<a name="10"></a></h2><p>We'll quickly re-implement the above, with multiple continuous and discrete states. Let's use <tt>p</tt> for <img src="examples_odehybrid_eq06613056106014670631.png" alt="$x_1$"> and <tt>v</tt> for <img src="examples_odehybrid_eq03385156042344747669.png" alt="$x_2$">.</p><pre class="codeinput">ode = @(t, p, v, u, i) deal(v, <span class="keyword">...</span><span class="comment">              % dp/dt (same as before)</span>
+                            2 * p + u + 1);     <span class="comment">% dv/dt (same as before)</span>
+de  = @(t, p, v, u, i) deal(p, <span class="keyword">...</span><span class="comment">              % No change to p</span>
+                            v, <span class="keyword">...</span><span class="comment">              % No change to v</span>
+                            -8*p - 4*v - i, <span class="keyword">...</span><span class="comment"> % Update input</span>
+                            i + 0.5 * p);       <span class="comment">% Update integrator</span>
+dt  = 0.1;                                      <span class="comment">% Discrete eq. time step</span>
+ts  = [0 5];                                    <span class="comment">% From 0 to 5s</span>
+x0  = {1; 0};                                   <span class="comment">% Initial continuous states</span>
+d0  = {0, 0};                                   <span class="comment">% Initial discrete states</span>
+[t, p, v, td, u, i] = odehybrid(@ode45, ode, de, dt, ts, x0, d0);
+plot(t, [p, v], td, [u, i], <span class="string">'.'</span>); xlabel(<span class="string">'Time'</span>);
+legend(<span class="string">'p'</span>, <span class="string">'v'</span>, <span class="string">'u'</span>, <span class="string">'\int p(t)/2 dt'</span>);
+
+num_steps = length(t);
+</pre><img vspace="5" hspace="5" src="examples_odehybrid_03.png" alt=""> <p>Note how all of states are inputs to <tt>ode</tt> and <tt>de</tt> (and therefore must be outputs). Also notice how all of the states are output from <tt>odehybrid</tt>.</p><h2>Using Structs for States<a name="12"></a></h2><p>Aside from using multiple states, we can also use states that aren't simple scalars or vectors. For instance, we can use structs. As a system begins to have many subsystems, using structs is a good way to keep track of scope for all of the states. For instance, we might have <tt>car_sw.cruise_ctrl.speed</tt> and <tt>car_sw.monitor.lane_watcher</tt>, clearly keeping variables scoped to specific systems. Plus, it's easy to hand off relevant substructs to the subsystems:</p><pre class="language-matlab">car_sw.cruise_ctrl = cruise_alg(car_sw.cruise_ctrl, current_speed);
+<span class="comment">% ...</span>
+car_sw.monitor = monitor_alg(car_sw.monitor, sensor_bus, ..);
+</pre><p>Let's continue our PID controller example, making the discrete state a structure with fields for <tt>u</tt> and <tt>i</tt>, and let's also make the continuous state a struct with fields for <tt>p</tt> and <tt>v</tt>.</p><pre class="codeinput">type <span class="string">example_odehybrid_structs</span>
+</pre><pre class="codeoutput">
+% A simple example of using structs with odehybrid.
+function example_odehybrid_structs()
+
+    dt = 0.1;                    % Discrete eq. time step
+    ts = [0 5];                  % Simulation time
+    x0 = struct('p', 1, 'v', 0); % Initial continuous states
+    d0 = struct('u', 0, 'i', 0); % Initial discrete states
+    
+    % Simulate.
+    [t, sig, td, ctrl] = odehybrid(@ode45, @ode, @de, dt, ts, x0, d0);
+    
+    % Plot.
+    plot(t, [sig.p], t, [sig.v], td, [ctrl.u], '.', td, [ctrl.i], '.');
+    xlabel('Time');
+    legend('p', 'v', 'u', '\int p(t)/2 dt');
+    
+end
+
+% Continuous differential equation
+function dsdt = ode(t, signal, controller) %#ok&lt;INUSL&gt;
+    dsdt.p = signal.v;
+    dsdt.v = 2 * signal.p + controller.u + 1;
+end
+
+% Discrete update equation
+function [signal, controller] = de(t, signal, controller) %#ok&lt;INUSL&gt;
+    controller.u = -8 * signal.p - 4*signal.v - controller.i;
+    controller.i = controller.i + 0.5 * signal.p;
+end
+</pre><p>Updating the structures with the discrete update equation is clear (simply update the structs and return them). For the continuous state, we return structs with the same structure, but the values they contain are the time derivatives. Notice in the above out the output struct of the differential equation is called <tt>dsdt</tt> for "d-signal-by-dt". Its <tt>p</tt> field is really <img src="examples_odehybrid_eq08268459927501053355.png" alt="$dp/dt$"> and similarly its <tt>v</tt> field is really <img src="examples_odehybrid_eq17693446393934962769.png" alt="$dv/dt$">. In this way, all of the values in the input struct can be correctly updated with the corresponding derivatives.</p><pre class="codeinput">example_odehybrid_structs();
+</pre><img vspace="5" hspace="5" src="examples_odehybrid_04.png" alt=""> <h2>Using Other Types<a name="16"></a></h2><p>We can mix and match states of different types, including numeric types, structs, and cell arrays. Here's an example set of discrete states with all three:</p><pre class="language-matlab">d0 = {magic(3),                  <span class="comment">% 2D array</span>
+      struct(<span class="string">'mode'</span>, 1),         <span class="comment">% struct</span>
+      {5, [1 2 3], [1 2; 3 4]}}; <span class="comment">% cell array</span>
+</pre><p>Numeric types can also have any data type, e.g. int16, and this will be handled appropriately.</p><p>There is one important thing to note: the size and structure of these states can't change during the simulation. For instance, if one state were a string for the status, e.g., 'online', and the status were changed in the simulation to 'offline' (a different number of characters), there would be an error. Instead, one can either use numbers to represent online or offline or one could pad online with an extra space, i.e., 'online '.</p><p>For advanced types: if custom objects are desired for the states, those can be used to. However, they must have a <tt>state_to_vector</tt> method that returns a complete representation of the internal state numerically and a <tt>vector_to_state</tt> method that reconstructs the object from a vector. See those two function respectively.</p><h2>Using Logging<a name="17"></a></h2><p>Good simulations allow a lot of analysis. So far, we've only been able to get states out of the system and to analyze those states, but what if there were some intermediate variable we wanted to display? We could use MATLAB's debugger to observe their values at each sample, but it's often better to store all of those values and examine them afterwards -- logging. These tools include a logging mechanism called TimeSeriesLogger (it has nothing to do with the <tt>timeseries</tt> class in MATLAB). It's pretty easy to use. Let's take a look.</p><pre class="codeinput">log = TimeSeriesLogger(); <span class="comment">% Create it.</span>
+<span class="keyword">for</span> t = 0:100
+    log.add(<span class="string">'signal 1'</span>, t, randn());
+    log.add(<span class="string">'signal 2'</span>, t, 10*rand());
+<span class="keyword">end</span>
+log.plot();
+</pre><img vspace="5" hspace="5" src="examples_odehybrid_05.png" alt=""> <p>There are many additional options (see the <tt>help TimeSeriesLogger</tt>), but let's focus on how to use this with our simulation. We'll continue to modify the struct example above. This will consist basically of:</p><div><ol><li>Creating a TimeSeriesLogger and passing it to <tt>odehybrid</tt>.</li><li>Adding a <tt>log</tt> input to the <tt>ode</tt> and <tt>de</tt> functions.</li><li>Adding some logging inside those functions.</li><li>Plotting the results when the simulation is done.</li></ol></div><p>Here are the new bits:</p><pre class="language-matlab">log = TimeSeriesLogger();     <span class="comment">% Create the logger.</span>
+<span class="comment">% ...</span>
+[t, sig, td, ctrl] = odehybrid(@ode45, @ode, @de, dt, ts, x0, d0, <span class="keyword">...</span>
+                               [], log);
+<span class="comment">% ...</span>
+</pre><pre class="language-matlab"><span class="keyword">function</span> dsdt = ode(t, signal, controller, log) <span class="keyword">...</span>
+    <span class="comment">% ...</span>
+    <span class="keyword">if</span> nargin &gt;= 4
+        log.add(<span class="string">'acceleration'</span>, t, dsdt.v);
+    <span class="keyword">end</span>
+    <span class="comment">% ...</span>
+</pre><pre class="language-matlab"><span class="keyword">function</span> [signal, controller] = de(t, signal, controller, log)
+    <span class="comment">%...</span>
+    log.add(<span class="string">'sampled v'</span>, t, signal.v);
+    <span class="comment">% ...</span>
+</pre><p>Here's the complete simulator:</p><pre class="codeinput">type <span class="string">example_odehybrid_logging</span>
+</pre><pre class="codeoutput">
+% A simple example of using logging with odehybrid.
+function example_odehybrid_logging()
+
+    dt  = 0.1;                    % Discrete eq. time step
+    ts  = [0 5];                  % Simulation time
+    x0  = struct('p', 1, 'v', 0); % Initial continuous states
+    d0  = struct('u', 0, 'i', 0); % Initial discrete states
+    log = TimeSeriesLogger();     % Create the logger.
+    
+    % Simulate.
+    [t, sig, td, ctrl] = odehybrid(@ode45, @ode, @de, dt, ts, x0, d0, ...
+                                   [], log);
+    
+    % Plot.
+    plot(t, [sig.p], t, [sig.v], td, [ctrl.u], '.', td, [ctrl.i], '.');
+    xlabel('Time');
+    legend('p', 'v', 'u', '\int p(t)/2 dt');
+    
+    % Add log output.
+    log.plot();
+    xlabel('Time');
+    
+end
+
+% Continuous differential equation
+function dsdt = ode(t, signal, controller, log)
+
+    % Calculate the derivatives.
+    dsdt.p = signal.v;
+    dsdt.v = 2 * signal.p + controller.u + 1;
+    
+    % Log the acceleration. We *must* check to see if the log is passed in;
+    % it won't always be passed in.
+    if nargin &gt;= 4
+        log.add('acceleration', t, dsdt.v);
+    end
+    
+end
+
+% Discrete update equation
+function [signal, controller] = de(t, signal, controller, log)
+
+    % Update the discrete state.
+    controller.u = -8 * signal.p - 4*signal.v - controller.i;
+    controller.i = controller.i + 0.5 * signal.p;
+    
+    % Log the velocity as it was sampled. Logs are always passed to the
+    % discrete update functions, so we don't explicitly need to check.
+    log.add('sampled v', t, signal.v);
+    
+end
+</pre><p>And here's the output from the log.</p><pre class="codeinput">example_odehybrid_logging();
+</pre><img vspace="5" hspace="5" src="examples_odehybrid_06.png" alt=""> <p>There we have it. Note that the log <i>won't</i> always be passed in to the ODE. The reason for this is that sometimes the continuous propagators take a large time step, see that the step was too large, and go back and take a smaller time step. The first step is discarded. Because we don't want phantom data points from discarded time steps, <tt>odehybrid</tt> doesn't pass in the logger until the steps are finalized.</p><p><center><h1>Beyond the Basics</h1></center></p><h2>Interpolation<a name="23"></a></h2><p>There's a quick detail that must be pointed out about discrete-time updates. Because these update states <i>directly</i>, those states change <i>instantaneously</i>. That is, at say, t=3 seconds, there's the "original" value of a state ("just to the left" of 3, usually denoted as <img src="examples_odehybrid_eq17676824639475809093.png" alt="$3^-$">). Then there's the updated value at <img src="examples_odehybrid_eq01891744225594948285.png" alt="$3^+$">. So our time and state history might look like this:</p><pre class="codeinput">t = [2.76, 2.91, 3,   3,   3.12].';
+x = [0.2,  0.3,  0.4, 1.1, 1.2].';
+
+clf();
+plot(t, x, <span class="string">'.-'</span>);
+</pre><img vspace="5" hspace="5" src="examples_odehybrid_07.png" alt=""> <p>If we were to try to interpolate this data with <tt>interp1</tt>, it wouldn't work, because it wouldn't know how to handle the doubled 3.</p><p>Therefore, <tt>odehybrid</tt> includes a function called <tt>interpd</tt> (for "interpolate, discrete") which can handle this type of thing. Simply specific whether the "-" or "+" values are desired during interpolation. For <tt>+</tt>, all interpolation between <tt>t</tt> = 2.91 and 3 will use [0.3 0.4] while exactly at 3 the datapoint will be 1.7, and interpolation afterwards uses [1.7 and 1.8]. For <tt>-</tt>, it's the same, except the value right at 3 is 0.4.</p><pre class="codeinput"><span class="comment">% Resample |t| and |x| at |ti| using the "right" value:</span>
+ti = (2.8:0.1:3.1).';
+xi = interpd(t, x, ti, <span class="string">'+'</span>).'
+xi = interpd(t, x, ti, <span class="string">'-'</span>).'
+</pre><pre class="codeoutput">xi =
+    0.2267    0.2933    1.1000    1.1833
+xi =
+    0.2267    0.2933    0.4000    1.1833
+</pre><h2>Other Propagators<a name="25"></a></h2><p>While <tt>ode23</tt> and <tt>ode45</tt>, etc., are robust and convenient tools representing some of what's best about MATLAB, <tt>odehybrid</tt> can expose a few pecularities. For instance, regardless of the dynamics, those functions will always take <i>at least</i> 41 steps to propagate (regardless of what the 'InitialStep' value is in the <tt>odeset</tt> structure). Since <tt>odehybrid</tt> uses <tt>ode45</tt> (or whatever propagator is passed in) between the discrete steps, this means there will be at least 41 time steps representing the continuous dynamics between the discrete steps. Ouch! That can be way too much. For this reason, <tt>odehybrid</tt> includes a few of propagators with the same interface as the <tt>odefun</tt> functions. Perhaps the easiest is <tt>rkadapt</tt>, which is essentially a drop-in replacement for <tt>@ode45</tt> in the examples above.</p><pre class="codeinput">[t, p, v, td, u, i] = odehybrid(@rkadapt, ode, de, dt, ts, x0, d0);
+
+fprintf(<span class="string">'This simulation took %.0f%% of the steps required by ode45.\n'</span>,<span class="keyword">...</span>
+        100*length(t)/num_steps);
+</pre><pre class="codeoutput">This simulation took 33% of the steps required by ode45.
+</pre><p>The <tt>ode45</tt> and <tt>rkadapt</tt> functions are <i>adaptive</i> time-step propagators. This means they "figure out" an appropriate time step to take (see the linked article on how simulations work). However, if one knows a good time step, that can be specified with a fixed-step solver. MATLAB doesn't include any of these, so <tt>odehybrid</tt> includes two: <tt>rk4</tt>, the most common fixed-step propagator, and <tt>rkfixed</tt>, which can implement any fixed-step Runge-Kutta method given the appropriate arguments. We'll show an example using <tt>rk4</tt>. Note that the time step is specified witht he <tt>odeset</tt> structure.</p><pre class="codeinput">options = odeset(<span class="string">'MaxStep'</span>, dt);
+[t, p, v, td, u, i] = odehybrid(@rk4, ode, de, dt, ts, x0, d0, options);
+
+fprintf(<span class="string">'This simulation took %.0f%% of the steps required by ode45.\n'</span>,<span class="keyword">...</span>
+        100*length(t)/num_steps);
+</pre><pre class="codeoutput">This simulation took 22% of the steps required by ode45.
+</pre><h2>Multiple Discrete Updates<a name="27"></a></h2><p>Using multiple discrete update equations is as passing multiple function (in a cell array) in for <tt>de</tt> and passing their corresponding time steps in for <tt>dt</tt>. Here is a simple harmonic oscillator. The discrete updates only "sample" the oscillator, with the first update recording the "position" term and the second update recording the "velocity" term. Each discrete update must take in and return all continuous and discrete states (just like for the case with a single discrete update equation).</p><pre class="codeinput">ode = @(t, x, p, v) [0 1; -1 0] * x;         <span class="comment">% Differential equation</span>
+de  = {@(t, x, p, v) deal(x, x(1), v); <span class="keyword">...</span><span class="comment">   % Discrete update equation 1</span>
+       @(t, x, p, v) deal(x, p, x(2))};      <span class="comment">% Discrete update equation 2</span>
+dt  = [0.1, 0.15];                           <span class="comment">% Discrete eq. time steps</span>
+ts  = [0 2*pi];                              <span class="comment">% From 0 to 5s</span>
+x0  = [1; 0];                                <span class="comment">% Initial continuous state</span>
+y0  = {0, 0};                                <span class="comment">% Initial discrete state</span>
+
+[t, x, ty, y1, y2] = odehybrid(@rkfixed, ode, de, dt, ts, x0, y0);
+
+clf();
+plot(t, x, <span class="string">'.-'</span>);
+hold <span class="string">on</span>;
+stairs(ty, [y1 y2], <span class="string">':'</span>);
+hold <span class="string">off</span>;
+legend(<span class="string">'Position'</span>, <span class="string">'Velocity'</span>, <span class="string">'Position (0.1s)'</span>, <span class="string">'Velocity (0.15s)'</span>);
+xlabel(<span class="string">'Time (s)'</span>);
+</pre><img vspace="5" hspace="5" src="examples_odehybrid_08.png" alt=""> <p><center><h1>Advanced Options</h1></center></p><h2>Using an OutputFcn<a name="29"></a></h2><p>The <tt>ode45</tt> function allows one to specify an <tt>OutputFcn</tt> to be called when the states are updated. This function can, e.g. update plots or determine if the simulation should stop (if it returns anything but 0). <tt>odehybrid</tt> allows this to pass through, but with all of the arguments separated just like for the ODE and DEs. For instance, here's a simple <tt>OutputFcn</tt>:</p><pre class="codeinput">type <span class="string">example_odehybrid_outputfcn</span>;
+</pre><pre class="codeoutput">
+% A simple example OutputFcn used with examples_odehybrid.
+function status = example_odehybrid_outputfcn(t, x, p, v, flag)
+
+    % Return true to terminate the propagation.
+    status = 0;
+    
+    switch flag
+        
+        % On init, we receive the time span and init. states.
+        case 'init'
+            fprintf('Simulating from %fs to %fs.\n', t);
+            
+        % When done, times and states are all empty.
+        case 'done'
+            fprintf('Finished the simulation.\n');
+            
+        % Otherwise, we receive 1 or more samples.
+        otherwise
+            status = x(1) &lt; 0; % End the simulation with x(1) &lt; 0.
+            
+    end
+    
+end
+</pre><p>Let's add this to the simulation above.</p><pre class="codeinput">options = odeset(<span class="string">'OutputFcn'</span>, @example_odehybrid_outputfcn);
+[t, x, ty, y1, y2] = odehybrid(@rkfixed, ode, de, dt, ts, x0, y0, options);
+</pre><pre class="codeoutput">Simulating from 0.000000s to 6.283185s.
+Finished the simulation.
+</pre><p>Note that the simulation stopped when <tt>x(1)</tt> fell below 0.</p><pre class="codeinput">plot(t, x);
+xlabel(<span class="string">'Time (s)'</span>);
+</pre><img vspace="5" hspace="5" src="examples_odehybrid_09.png" alt=""> <h2>Using Events<a name="32"></a></h2><p>Another useful property of MATLAB's ODE suite is events. These are used to "narrow in" on a specific occurrence. To use event, one write an event function, which should return a continuous value. The propagator will search for the time at which that value crosses 0. For instance, suppose one wanted a simulation to end when a parachutist lands on the ground. Then one could write an event function that returns her height above the ground. The propagator will take small time steps towards the moment when the height achieves 0. The event function should also return two more pieces of information: if the simulation is to stop when the event occurs and the direction of the zero crossing. See <tt>doc odeset</tt> for more on Events.</p><p>In <tt>odehybrid</tt>, events are like those for <tt>ode45</tt>, but take the full set of states as arguments, just like the ODE, DE, and <tt>OutputFcn</tt>. This function will cause propagation to terminate when the velocity (<tt>x(2)</tt>) goes to zero from below.</p><pre class="codeinput">type <span class="string">example_odehybrid_eventfcn</span>
+</pre><pre class="codeoutput">
+% A simple example Event function used with examples_odehybrid.
+function [h, t, d] = example_odehybrid_eventfcn(t, x, p, v)
+    h = x(2); % An "event" occurs when the velocity is 0.
+    t = true; % Terminate on event; stop the simulation when h=0.
+    d = 1;    % Trigger when going positive from negative
+end
+</pre><p>Let's add this to the simulation above. Note that we can only use events with the ODE suite functions. Also, note all the "event" outputs.</p><pre class="codeinput">options = odeset(<span class="string">'Events'</span>, @example_odehybrid_eventfcn);
+[t, x, ty, y1, y2, te, xe, y1e, y2e, ie] = <span class="keyword">...</span>
+                       odehybrid(@ode45, ode, de, dt, ts, x0, y0, options);
+plot(t, x, <span class="keyword">...</span>
+     te, xe, <span class="string">'o'</span>);
+xlabel(<span class="string">'Time (s)'</span>);
+</pre><img vspace="5" hspace="5" src="examples_odehybrid_10.png" alt=""> <p>An event function can return vectors instead of scalars, representing multiple possible zero-crossings. The triggering event index is output in <tt>ie</tt>.</p><h2>Using Structs for Output<a name="35"></a></h2><p>If having eight output arguments is getting to be too many, we can instead use the struct output form.</p><pre class="codeinput">sol = odehybrid(@ode45, ode, de, dt, ts, x0, y0, options)
+</pre><pre class="codeoutput">sol = 
+      t: [463x1 double]
+     yc: {[463x2 double]}
+     td: [42x1 double]
+     yd: {[1]  [0.9950]}
+     te: 3.1416
+    yce: {[-1.0000 2.8363e-14]}
+    yde: {[-0.9991]  [-0.1411]}
+     ie: 1
+</pre><p>The struct has a field for each output of <tt>odehybrid</tt>, with the states grouped into cell arrays.</p><p><center><h1>Comments</h1></center></p><h2>Notes on Speed<a name="38"></a></h2><p>Most folks would like their simulations to run fast. Here are a few things to consider.</p><p>The first thing, which doesn't really have to do with the tools at all, is to make sure the algorithms make sense. For instance, are you simulating a stiff subsystem in the middle of an otherwise loose system (a part with a very fast reaciton time compared to other parts)? That likely causes the entire simulation to run slowly, and often for the purpose of the larger simulation, the stiffest systems can be replaced with functional equivalents (like a table lookup).</p><p>The next most important thing is to make the MATLAB code representing the ODE and the DE as fast as possible. See the MATLAB documentation under "Advaned Software Development &gt; Performance and Memory &gt; Code Performance" (R2014a).</p><p>Choosing the right continuous propagator also makes a large difference. While <tt>ode45</tt> is usually the best place to start, there are better choices are for different types of problems. For MATLAB's ODE propagators, see the MATLAB documentation under "Mathematics &gt; Numerical Integration... &gt; Ordinary Differential Equations". If MATLAB's propagators are taking an excessive number of steps for the continuous-time portions (as above and as is common for control problems), consider using the included <tt><a href="matlab:doc('rk4')">rk4</a></tt> or <tt><a href="matlab:doc('rkfixed')">rkfixed</a></tt> functions. One can also write one's own propagator or pass any Runge-Kutta method's details to <tt>rkfixed</tt> for an instant, custom propagator.</p><p>Using non-numeric types (like structs or cell arrays) for states is convenient and can dramatically reduce development and debugging time. However, there is naturally a speed penalty. How big that penalty is depends the complexity of the state and the functions using the state. In the author's simulations, it's often only about 20% of the overall simulation time, which is a bargain considering the convenience. This should be a last resort.</p><p>Finally, logging naturally adds overhead as well. However, some care was taken in minimize the performance impact from logging, and it's often a smaller concern even than using non-numeric types -- less than 20% of the run time. For large numberes of simulations for which detailed analysis won't be necessary, one can simply turn logging off (pass <tt>[]</tt> for the <tt>log</tt> argument).</p><p><center><h1>Summary</h1></center></p><h2>Summary<a name="40"></a></h2><p>We've covered a lot of ground, including how to set up continuous and discrete simulations, use complex states, add logging, work with the results, and achieve better speed. At this point, a careful reader should be able to start making simulations with <tt>odehybrid</tt>. However, this is not the end of the documentation; it's just a jumping off point. For more info, see:</p><div><ul><li><a href="http://www.anuncommonlab.com/doc/odehybrid/">The online documentation</a></li><li>Help for individual functions (e.g., <tt>doc TimeSeriesLogger</tt>)</li><li><a href="http://www.anuncommonlab.com/articles/how-simulations-work">How simulations work</a></li></ul></div><p class="footer"><br><a href="http://www.mathworks.com/products/matlab/">Published with MATLAB&reg; R2015a</a><br></p></div><!--
+##### SOURCE BEGIN #####
+%% Examples of |odehybrid|
+%
+% This script contains numerous examples of using |odehybrid| to simulate
+% continuous and discrete systems, from a simple example to more complex 
+% examples with non-vector states and logging, introducing all primary 
+% features of |odehybrid|. It's meant for viewing directly in MATLAB by 
+% clicking "Publish" in the editor or entering:
+%
+%   home = fileparts(which('examples_odehybrid'));
+%   web(fullfile(home, 'html', 'examples_odehybrid.html'));
+% 
+% at the command line. This will run the code and open the result as HTML.
+% 
+% For additional discussion, see:
+% 
+% * Online documentation (<http://www.anuncommonlab.com/doc/odehybrid/>)
+% * Discussion of how simulations work (<http://www.anuncommonlab.com/articles/how-simulations-work/>)
+% * Motivations behind the tool (<http://www.anuncommonlab.com/blog/a-simulation-engine/>)
+%
+% Copyright 2014 An Uncommon Lab
+
+%%
+% <html><center><h1>Basics</h1></center></html>
+
+%% Short and Direct
+% Let's say we have a continuous system in the form:
+%
+% $$\dot{x}_c(t) = f_c(t, x_c(t), x_d(t))$$
+%
+% where $x_c$ is the continuous state and $x_d$ is some state updated at a 
+% discrete time step as
+%
+% $$x_d(t+\Delta t) = f_d(t, x_c(t), x_d(t))$$
+% 
+% Here, we'll give a quick embodiment of this system to show how we can
+% simulate it from its initial condition over time. Afterwards, we'll go
+% more into how it works.
+% 
+% Let |ode|, |de|, |dt|, |x|, and |u| be $f_c$, $f_d$, $\Delta t$, $x_c$, 
+% and $x_d$, respectively.
+
+ode = @(t, x, u) [0 1; 2 0] * x + [0; 1] * u;  % Differential equation
+de  = @(t, x, u) deal(x, -[8 4] * x);          % Discrete update equation
+dt  = 0.1;                                     % Discrete eq. time step
+ts  = [0 5];                                   % From 0 to 5s
+x0  = [1; 0];                                  % Initial continuous state
+u0  = 0;                                       % Initial discrete state
+[t, x, tu, u] = odehybrid(@ode45, ode, de, dt, ts, x0, u0); % Simulate!
+plot(t, x, tu, u, '.'); xlabel('Time');                     % Plot 'em.
+legend('x_1', 'x_2', 'u', 'Location', 'se');                % Label 'em.
+
+%%
+% This was really quick. Notice how the discrete system is updated 
+% regularly every 0.1s, while the continuous system has many more
+% intermediate steps.
+% 
+% Let's look at what we did. First, it's clear that $f_c$ represents 
+% (suppressing the dependence on time):
+% 
+% $$
+%     \left[ \begin{array}{c} \dot{x}_1 \\ \dot{x}_2 \end{array} \right]
+%   =   \left[ \begin{array}{c c} 0 & 1 \\ 2 & 0 \end{array} \right]
+%       \left[ \begin{array}{c} x_1 \\ x_2 \end{array} \right]
+%     + \left[ \begin{array}{c} 0 \\ 1 \end{array} \right]
+%       u
+% $$
+%
+% The discrete state is $u$ and can be thought of as a controller of this 
+% unstable system, guiding it towards [0, 0] (without $u$, the system would
+% diverge from [0, 0]). It's updated at 10Hz according to $f_d$ which is 
+% just:
+%
+% $$
+%   u = \left[ \begin{array}{c c} -8 & -4 \end{array} \right] 
+%       \left[ \begin{array}{c} x_1 \\ x_2 \end{array} \right]
+% $$
+%
+% The second input to the |de| function is the |u| state, but the current
+% |u| won't depend on the last |u|, so it's unused.
+%
+% Notice that the discrete update equation actually has _two_ outputs (the
+% |deal| function maps argument 1 to the first output and argument 2 to the
+% second). This is because |odehybrid| allows the discrete update equation
+% to _also_ update the continuous state as well. This is useful for very
+% fast phenomena, like a quick burst of rocket power to change a
+% satellite's orbit. In MATLAB code, we have:
+% 
+%   [x_c, x_d] = de(t, x_c, x_d);
+%
+% Note that, despite that this example is using linear systems, that has
+% nothing to do with odehybrid. The continuous and discrete equations can
+% be anything (nonlinear, stochastic, user inputs, etc.).
+%
+% For the readers with controls backgrounds, this is clearly a discrete 
+% proportional-derivative controller for the unstable linear system.
+% 
+% That's all there is to setting up this basic system, so let's make things
+% more interesting.
+
+%% Multiple States
+% As state vectors become large, they become unwieldy. It's difficult to
+% remember what the 10th state is, or the 2031st state. For this reason,
+% |odehybrid| allows the use of individual states as individual arguments
+% into the ODE and DE functions.
+%
+% Let's reuse the above example, but let's add something to our discrete
+% controller. Instead of a proportional-derivative controllers, let's make
+% it a proportional-integral-derivative control. Now we'll have two
+% discrete states to keep track of: the input, |u|, and the integral term,
+% |i|, that gets updated every step.
+%
+% $$
+%   u = \left[ \begin{array}{c c c} -8 & -4 & -1 \end{array} \right] 
+%       \left[ \begin{array}{c} x_1 \\ x_2 \\ i \end{array} \right]
+% $$
+%
+% That |i| term is updated as:
+%
+% $$
+%   i = i + \frac{1}{2}x_1
+% $$
+%
+% To use multiple states, we'll need them to be inputs to our |ode| and
+% |de| functions. Plus, the |de| function will have to output the new
+% discrete state.
+%
+% We'll also add a disturbance to the model so the integral term has
+% something to account for. We'll just tack |+ [0; 1]| onto the end.
+%
+% Here's the new |ode| (the final input is the integral term, i, but it 
+% doesn't need it): 
+
+ode = @(t, x, u, i)   [0 1; 2 0] * x ...        % Continuous system
+                    + [0; 1] * u ...            %   with feedback control
+                    + [0; 1];                   %   and with a disturbance
+                
+%%
+% The new |de| is easy too (it uses the integral term, but not the last
+% |u|):
+
+de  = @(t, x, u, i) deal(x, ...                 % No change to cont. state
+                         -[8 4 1] * [x; i], ... % Update input
+                         i + 0.5 * x(1));       % Update integrator
+
+%%
+% The options are the same:
+                     
+dt  = 0.1;                                      % Discrete eq. time step
+ts  = [0 5];                                    % From 0 to 5s
+x0  = [1; 0];                                   % Initial continuous state
+
+%%
+% Two say we have two separate discrete states, we use a cell array for the
+% discrete state and put each initial value in the appropriate place.
+d0  = {0, 0};                                   % Initial discrete states
+
+%%
+% The discrete states are output separately:
+
+[t, x, td, u, i] = odehybrid(@ode45, ode, de, dt, ts, x0, d0); % Simulate!
+plot(t, x, td, [u, i], '.'); xlabel('Time');                   % Plot 'em.
+legend('x_1', 'x_2', 'u', '\int x_1(t)/2 dt');                 % Label 'em.
+
+%%
+% That's still pretty easy to implement. We can expand the continuous
+% states too. The important part is that the |ode| has the form:
+%
+%   [x_dot_1, x_dot_2, ..] = ode(t, x1, x2, .., xd1, xd2, ..);
+%
+% And the |de| has the form:
+% 
+%   [x1, x2, .., xd1, xd2, ..] = de(t, x1, x2, .., xd1, xd2, ..);
+%
+% After simulation is done, |odehybrid| will output:
+%
+%   [t, x1, x2, .., td, xd1, xd2, ..] = odehybrid(@ode45, ...
+%                                                 ode, de, dt, ts, ...
+%                                                 {x1, x2, ...}, ...
+%                                                 {xd1, xd2, ...});
+%                                                 
+
+%% Multiple Continuous and Discrete States
+% We'll quickly re-implement the above, with multiple continuous and
+% discrete states. Let's use |p| for $x_1$ and |v| for $x_2$.
+
+ode = @(t, p, v, u, i) deal(v, ...              % dp/dt (same as before)
+                            2 * p + u + 1);     % dv/dt (same as before)
+de  = @(t, p, v, u, i) deal(p, ...              % No change to p
+                            v, ...              % No change to v
+                            -8*p - 4*v - i, ... % Update input
+                            i + 0.5 * p);       % Update integrator
+dt  = 0.1;                                      % Discrete eq. time step
+ts  = [0 5];                                    % From 0 to 5s
+x0  = {1; 0};                                   % Initial continuous states
+d0  = {0, 0};                                   % Initial discrete states
+[t, p, v, td, u, i] = odehybrid(@ode45, ode, de, dt, ts, x0, d0);
+plot(t, [p, v], td, [u, i], '.'); xlabel('Time');
+legend('p', 'v', 'u', '\int p(t)/2 dt');
+
+num_steps = length(t);
+
+%%
+% Note how all of states are inputs to |ode| and |de| (and therefore must
+% be outputs). Also notice how all of the states are output from
+% |odehybrid|.
+
+%% Using Structs for States
+% Aside from using multiple states, we can also use states that aren't
+% simple scalars or vectors. For instance, we can use structs. As a
+% system begins to have many subsystems, using structs is a good way to
+% keep track of scope for all of the states. For instance, we might have
+% |car_sw.cruise_ctrl.speed| and |car_sw.monitor.lane_watcher|, clearly
+% keeping variables scoped to specific systems. Plus, it's easy to hand off
+% relevant substructs to the subsystems:
+%
+%   car_sw.cruise_ctrl = cruise_alg(car_sw.cruise_ctrl, current_speed);
+%   % ...
+%   car_sw.monitor = monitor_alg(car_sw.monitor, sensor_bus, ..);
+
+%%
+% Let's continue our PID controller example, making the discrete state a
+% structure with fields for |u| and |i|, and let's also make the continuous
+% state a struct with fields for |p| and |v|.
+
+type example_odehybrid_structs
+
+%%
+% Updating the structures with the discrete update equation is clear
+% (simply update the structs and return them). For the continuous state,
+% we return structs with the same structure, but the values they contain
+% are the time derivatives. Notice in the above out the output struct of
+% the differential equation is called |dsdt| for "d-signal-by-dt". Its |p|
+% field is really $dp/dt$ and similarly its |v| field is really $dv/dt$. In
+% this way, all of the values in the input struct can be correctly updated 
+% with the corresponding derivatives.
+
+%%
+example_odehybrid_structs();
+
+%% Using Other Types
+% We can mix and match states of different types, including numeric types,
+% structs, and cell arrays. Here's an example set of discrete states with
+% all three:
+%
+%   d0 = {magic(3),                  % 2D array
+%         struct('mode', 1),         % struct
+%         {5, [1 2 3], [1 2; 3 4]}}; % cell array
+%
+% Numeric types can also have any data type, e.g. int16, and this will be
+% handled appropriately.
+%
+% There is one important thing to note: the size and structure of these
+% states can't change during the simulation. For instance, if one state
+% were a string for the status, e.g., 'online', and the status were changed
+% in the simulation to 'offline' (a different number of characters), there
+% would be an error. Instead, one can either use numbers to represent
+% online or offline or one could pad online with an extra space, i.e.,
+% 'online '.
+%
+% For advanced types: if custom objects are desired for the states, those 
+% can be used to. However, they must have a |state_to_vector| method that 
+% returns a complete representation of the internal state numerically and a
+% |vector_to_state| method that reconstructs the object from a vector. See
+% those two function respectively.
+
+%% Using Logging
+% Good simulations allow a lot of analysis. So far, we've only been able to
+% get states out of the system and to analyze those states, but what if
+% there were some intermediate variable we wanted to display? We could use
+% MATLAB's debugger to observe their values at each sample, but it's often
+% better to store all of those values and examine them afterwards REPLACE_WITH_DASH_DASH
+% logging. These tools include a logging mechanism called TimeSeriesLogger 
+% (it has nothing to do with the |timeseries| class in MATLAB). It's pretty
+% easy to use. Let's take a look.
+
+log = TimeSeriesLogger(); % Create it.
+for t = 0:100
+    log.add('signal 1', t, randn());
+    log.add('signal 2', t, 10*rand());
+end
+log.plot();
+
+%%
+% There are many additional options (see the |help TimeSeriesLogger|), but
+% let's focus on how to use this with our simulation. We'll continue to
+% modify the struct example above. This will consist basically of:
+%
+% # Creating a TimeSeriesLogger and passing it to |odehybrid|.
+% # Adding a |log| input to the |ode| and |de| functions.
+% # Adding some logging inside those functions.
+% # Plotting the results when the simulation is done.
+%
+% Here are the new bits:
+% 
+%   log = TimeSeriesLogger();     % Create the logger.
+%   % ...
+%   [t, sig, td, ctrl] = odehybrid(@ode45, @ode, @de, dt, ts, x0, d0, ...
+%                                  [], log);
+%   % ...
+% 
+%   function dsdt = ode(t, signal, controller, log) ...
+%       % ...
+%       if nargin >= 4
+%           log.add('acceleration', t, dsdt.v);
+%       end
+%       % ...
+% 
+%   function [signal, controller] = de(t, signal, controller, log)
+%       %...
+%       log.add('sampled v', t, signal.v);
+%       % ...
+
+%%
+% Here's the complete simulator:
+type example_odehybrid_logging
+
+%%
+% And here's the output from the log.
+example_odehybrid_logging();
+
+%%
+% There we have it. Note that the log _won't_ always be passed in to the
+% ODE. The reason for this is that sometimes the continuous propagators
+% take a large time step, see that the step was too large, and go back and
+% take a smaller time step. The first step is discarded. Because we don't
+% want phantom data points from discarded time steps, |odehybrid| doesn't
+% pass in the logger until the steps are finalized.
+
+%%
+% <html><center><h1>Beyond the Basics</h1></center></html>
+
+%% Interpolation
+% There's a quick detail that must be pointed out about discrete-time
+% updates. Because these update states _directly_, those states change
+% _instantaneously_. That is, at say, t=3 seconds, there's the "original"
+% value of a state ("just to the left" of 3, usually denoted as $3^-$).
+% Then there's the updated value at $3^+$. So our time and state history
+% might look like this:
+
+t = [2.76, 2.91, 3,   3,   3.12].';
+x = [0.2,  0.3,  0.4, 1.1, 1.2].';
+
+clf();
+plot(t, x, '.-');
+
+%%
+% If we were to try to interpolate this data with |interp1|, it wouldn't
+% work, because it wouldn't know how to handle the doubled 3.
+% 
+% Therefore, |odehybrid| includes a function called |interpd| (for
+% "interpolate, discrete") which can handle this type of thing. Simply
+% specific whether the "-" or "+" values are desired during interpolation.
+% For |+|, all interpolation between |t| = 2.91 and 3 will use [0.3 0.4] 
+% while exactly at 3 the datapoint will be 1.7, and interpolation
+% afterwards uses [1.7 and 1.8]. For |-|, it's the same, except the value
+% right at 3 is 0.4.
+
+% Resample |t| and |x| at |ti| using the "right" value:
+ti = (2.8:0.1:3.1).';
+xi = interpd(t, x, ti, '+').'
+xi = interpd(t, x, ti, '-').'
+
+%% Other Propagators
+% While |ode23| and |ode45|, etc., are robust and convenient tools
+% representing some of what's best about MATLAB, |odehybrid| can expose a
+% few pecularities. For instance, regardless of the dynamics, those 
+% functions will always take _at least_ 41 steps to propagate (regardless 
+% of what the 'InitialStep' value is in the |odeset| structure). Since 
+% |odehybrid| uses |ode45| (or whatever propagator is passed in) between 
+% the discrete steps, this means there will be at least 41 time steps 
+% representing the continuous dynamics between the discrete steps. Ouch! 
+% That can be way too much. For this reason, |odehybrid| includes a few of 
+% propagators with the same interface as the |odefun| functions. Perhaps 
+% the easiest is |rkadapt|, which is essentially a drop-in replacement for 
+% |@ode45| in the examples above.
+
+[t, p, v, td, u, i] = odehybrid(@rkadapt, ode, de, dt, ts, x0, d0);
+
+fprintf('This simulation took %.0f%% of the steps required by ode45.\n',...
+        100*length(t)/num_steps);
+
+%%
+% The |ode45| and |rkadapt| functions are _adaptive_ time-step propagators.
+% This means they "figure out" an appropriate time step to take (see the
+% linked article on how simulations work). However, if one knows a good
+% time step, that can be specified with a fixed-step solver. MATLAB doesn't
+% include any of these, so |odehybrid| includes two: |rk4|, the most common
+% fixed-step propagator, and |rkfixed|, which can implement any fixed-step 
+% Runge-Kutta method given the appropriate arguments. We'll show an example
+% using |rk4|. Note that the time step is specified witht he |odeset|
+% structure.
+
+options = odeset('MaxStep', dt);
+[t, p, v, td, u, i] = odehybrid(@rk4, ode, de, dt, ts, x0, d0, options);
+
+fprintf('This simulation took %.0f%% of the steps required by ode45.\n',...
+        100*length(t)/num_steps);
+
+%% Multiple Discrete Updates
+% Using multiple discrete update equations is as passing multiple function
+% (in a cell array) in for |de| and passing their corresponding time steps 
+% in for |dt|. Here is a simple harmonic oscillator. The discrete updates
+% only "sample" the oscillator, with the first update recording the
+% "position" term and the second update recording the "velocity" term. Each
+% discrete update must take in and return all continuous and discrete
+% states (just like for the case with a single discrete update equation).
+
+ode = @(t, x, p, v) [0 1; -1 0] * x;         % Differential equation
+de  = {@(t, x, p, v) deal(x, x(1), v); ...   % Discrete update equation 1
+       @(t, x, p, v) deal(x, p, x(2))};      % Discrete update equation 2
+dt  = [0.1, 0.15];                           % Discrete eq. time steps
+ts  = [0 2*pi];                              % From 0 to 5s
+x0  = [1; 0];                                % Initial continuous state
+y0  = {0, 0};                                % Initial discrete state
+
+[t, x, ty, y1, y2] = odehybrid(@rkfixed, ode, de, dt, ts, x0, y0);
+
+clf();
+plot(t, x, '.-');
+hold on;
+stairs(ty, [y1 y2], ':');
+hold off;
+legend('Position', 'Velocity', 'Position (0.1s)', 'Velocity (0.15s)');
+xlabel('Time (s)');
+
+%%
+% <html><center><h1>Advanced Options</h1></center></html>
+
+%% Using an OutputFcn
+% The |ode45| function allows one to specify an |OutputFcn| to be called
+% when the states are updated. This function can, e.g. update plots or 
+% determine if the simulation should stop (if it returns anything but 0).
+% |odehybrid| allows this to pass through, but with all of the arguments 
+% separated just like for the ODE and DEs. For instance, here's a simple
+% |OutputFcn|:
+
+type example_odehybrid_outputfcn;
+
+%%
+% Let's add this to the simulation above.
+options = odeset('OutputFcn', @example_odehybrid_outputfcn);
+[t, x, ty, y1, y2] = odehybrid(@rkfixed, ode, de, dt, ts, x0, y0, options);
+
+%%
+% Note that the simulation stopped when |x(1)| fell below 0.
+plot(t, x);
+xlabel('Time (s)');
+
+%% Using Events
+% Another useful property of MATLAB's ODE suite is events. These are used
+% to "narrow in" on a specific occurrence. To use event, one write an event
+% function, which should return a continuous value. The propagator will
+% search for the time at which that value crosses 0. For instance, suppose
+% one wanted a simulation to end when a parachutist lands on the ground.
+% Then one could write an event function that returns her height above the
+% ground. The propagator will take small time steps towards the moment when
+% the height achieves 0. The event function should also return two more
+% pieces of information: if the simulation is to stop when the event occurs
+% and the direction of the zero crossing. See |doc odeset| for more on
+% Events.
+%
+% In |odehybrid|, events are like those for |ode45|, but take the full set
+% of states as arguments, just like the ODE, DE, and |OutputFcn|. This
+% function will cause propagation to terminate when the velocity (|x(2)|)
+% goes to zero from below.
+
+type example_odehybrid_eventfcn
+
+%%
+% Let's add this to the simulation above. Note that we can only use events
+% with the ODE suite functions. Also, note all the "event" outputs.
+
+options = odeset('Events', @example_odehybrid_eventfcn);
+[t, x, ty, y1, y2, te, xe, y1e, y2e, ie] = ...
+                       odehybrid(@ode45, ode, de, dt, ts, x0, y0, options);
+plot(t, x, ...
+     te, xe, 'o');
+xlabel('Time (s)');
+
+%%
+% An event function can return vectors instead of scalars, representing
+% multiple possible zero-crossings. The triggering event index is output in
+% |ie|.
+
+%% Using Structs for Output
+% If having eight output arguments is getting to be too many, we can
+% instead use the struct output form.
+
+sol = odehybrid(@ode45, ode, de, dt, ts, x0, y0, options)
+
+%%
+% The struct has a field for each output of |odehybrid|, with the states 
+% grouped into cell arrays.
+
+%%
+% <html><center><h1>Comments</h1></center></html>
+
+%% Notes on Speed
+% Most folks would like their simulations to run fast. Here are a few
+% things to consider.
+% 
+% The first thing, which doesn't really have to do with the tools at all,
+% is to make sure the algorithms make sense. For instance, are you
+% simulating a stiff subsystem in the middle of an otherwise loose
+% system (a part with a very fast reaciton time compared to other parts)?
+% That likely causes the entire simulation to run slowly, and often for the
+% purpose of the larger simulation, the stiffest systems can be replaced 
+% with functional equivalents (like a table lookup).
+%
+% The next most important thing is to make the MATLAB code representing the
+% ODE and the DE as fast as possible. See the MATLAB documentation under
+% "Advaned Software Development > Performance and Memory > Code
+% Performance" (R2014a).
+% 
+% Choosing the right continuous propagator also makes a large difference.
+% While |ode45| is usually the best place to start, there are better 
+% choices are for different types of problems. For MATLAB's ODE 
+% propagators, see the MATLAB documentation under "Mathematics > Numerical 
+% Integration... > Ordinary Differential Equations". If MATLAB's
+% propagators are taking an excessive number of steps for the 
+% continuous-time portions (as above and as is common for control 
+% problems), consider using the included 
+% |<matlab:doc('rk4') rk4>| or |<matlab:doc('rkfixed') rkfixed>| functions.
+% One can also write one's own propagator or pass any Runge-Kutta method's
+% details to |rkfixed| for an instant, custom propagator.
+%
+% Using non-numeric types (like structs or cell arrays) for states is
+% convenient and can dramatically reduce development and debugging time. 
+% However, there is naturally a speed penalty. How big that penalty is
+% depends the complexity of the state and the functions using the state. In
+% the author's simulations, it's often only about 20% of the overall 
+% simulation time, which is a bargain considering the convenience. This
+% should be a last resort.
+% 
+% Finally, logging naturally adds overhead as well. However, some care was
+% taken in minimize the performance impact from logging, and it's often a
+% smaller concern even than using non-numeric types REPLACE_WITH_DASH_DASH less than 20% of the
+% run time. For large numberes of simulations for which detailed analysis
+% won't be necessary, one can simply turn logging off (pass |[]| for the
+% |log| argument).
+
+%%
+% <html><center><h1>Summary</h1></center></html>
+
+%% Summary
+% We've covered a lot of ground, including how to set up continuous and
+% discrete simulations, use complex states, add logging, work with the
+% results, and achieve better speed. At this point, a careful reader should
+% be able to start making simulations with |odehybrid|. However, this is 
+% not the end of the documentation; it's just a jumping off point. For more
+% info, see:
+% 
+% * <http://www.anuncommonlab.com/doc/odehybrid/ The online documentation>
+% * Help for individual functions (e.g., |doc TimeSeriesLogger|)
+% * <http://www.anuncommonlab.com/articles/how-simulations-work How simulations work>
+
+##### SOURCE END #####
+--></body></html>
\ No newline at end of file
diff --git a/Lib/various/odehybrid/html/examples_odehybrid.png b/Lib/various/odehybrid/html/examples_odehybrid.png
new file mode 100644
index 0000000..b054f6e
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_01.png b/Lib/various/odehybrid/html/examples_odehybrid_01.png
new file mode 100644
index 0000000..ced0da4
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_01.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_02.png b/Lib/various/odehybrid/html/examples_odehybrid_02.png
new file mode 100644
index 0000000..ddf58e5
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_02.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_03.png b/Lib/various/odehybrid/html/examples_odehybrid_03.png
new file mode 100644
index 0000000..64fc0cb
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_03.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_04.png b/Lib/various/odehybrid/html/examples_odehybrid_04.png
new file mode 100644
index 0000000..64fc0cb
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_04.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_05.png b/Lib/various/odehybrid/html/examples_odehybrid_05.png
new file mode 100644
index 0000000..5464558
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_05.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_06.png b/Lib/various/odehybrid/html/examples_odehybrid_06.png
new file mode 100644
index 0000000..a6a47d0
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_06.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_07.png b/Lib/various/odehybrid/html/examples_odehybrid_07.png
new file mode 100644
index 0000000..b229a5b
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_07.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_08.png b/Lib/various/odehybrid/html/examples_odehybrid_08.png
new file mode 100644
index 0000000..d899689
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_08.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_09.png b/Lib/various/odehybrid/html/examples_odehybrid_09.png
new file mode 100644
index 0000000..7706e8a
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_09.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_10.png b/Lib/various/odehybrid/html/examples_odehybrid_10.png
new file mode 100644
index 0000000..d151595
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_10.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_eq01891744225594948285.png b/Lib/various/odehybrid/html/examples_odehybrid_eq01891744225594948285.png
new file mode 100644
index 0000000..635b604
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_eq01891744225594948285.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_eq03385156042344747669.png b/Lib/various/odehybrid/html/examples_odehybrid_eq03385156042344747669.png
new file mode 100644
index 0000000..d3b6e15
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_eq03385156042344747669.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_eq03870024100092193585.png b/Lib/various/odehybrid/html/examples_odehybrid_eq03870024100092193585.png
new file mode 100644
index 0000000..e004eea
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_eq03870024100092193585.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_eq04503507910695345665.png b/Lib/various/odehybrid/html/examples_odehybrid_eq04503507910695345665.png
new file mode 100644
index 0000000..c68ed46
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_eq04503507910695345665.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_eq06613056106014670631.png b/Lib/various/odehybrid/html/examples_odehybrid_eq06613056106014670631.png
new file mode 100644
index 0000000..8faec40
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_eq06613056106014670631.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_eq06902086386518730424.png b/Lib/various/odehybrid/html/examples_odehybrid_eq06902086386518730424.png
new file mode 100644
index 0000000..720f8c5
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_eq06902086386518730424.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_eq07323985336366517770.png b/Lib/various/odehybrid/html/examples_odehybrid_eq07323985336366517770.png
new file mode 100644
index 0000000..9da5f60
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_eq07323985336366517770.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_eq07651498126111630170.png b/Lib/various/odehybrid/html/examples_odehybrid_eq07651498126111630170.png
new file mode 100644
index 0000000..d3b4500
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_eq07651498126111630170.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_eq07737725388692818491.png b/Lib/various/odehybrid/html/examples_odehybrid_eq07737725388692818491.png
new file mode 100644
index 0000000..4e9a3a2
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_eq07737725388692818491.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_eq08268459927501053355.png b/Lib/various/odehybrid/html/examples_odehybrid_eq08268459927501053355.png
new file mode 100644
index 0000000..e740bc7
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_eq08268459927501053355.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_eq08537354732563299936.png b/Lib/various/odehybrid/html/examples_odehybrid_eq08537354732563299936.png
new file mode 100644
index 0000000..6f2b850
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_eq08537354732563299936.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_eq11099118058511462397.png b/Lib/various/odehybrid/html/examples_odehybrid_eq11099118058511462397.png
new file mode 100644
index 0000000..2acde1b
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_eq11099118058511462397.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_eq11149857664895560545.png b/Lib/various/odehybrid/html/examples_odehybrid_eq11149857664895560545.png
new file mode 100644
index 0000000..344a1f0
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_eq11149857664895560545.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_eq11776305044305525613.png b/Lib/various/odehybrid/html/examples_odehybrid_eq11776305044305525613.png
new file mode 100644
index 0000000..1beba5e
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_eq11776305044305525613.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_eq14964292944995088614.png b/Lib/various/odehybrid/html/examples_odehybrid_eq14964292944995088614.png
new file mode 100644
index 0000000..c56749f
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_eq14964292944995088614.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_eq17676824639475809093.png b/Lib/various/odehybrid/html/examples_odehybrid_eq17676824639475809093.png
new file mode 100644
index 0000000..559ccb7
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_eq17676824639475809093.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_eq17693446393934962769.png b/Lib/various/odehybrid/html/examples_odehybrid_eq17693446393934962769.png
new file mode 100644
index 0000000..058bdd7
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_eq17693446393934962769.png differ
diff --git a/Lib/various/odehybrid/html/examples_odehybrid_eq17804952888730603842.png b/Lib/various/odehybrid/html/examples_odehybrid_eq17804952888730603842.png
new file mode 100644
index 0000000..b533ec4
Binary files /dev/null and b/Lib/various/odehybrid/html/examples_odehybrid_eq17804952888730603842.png differ
diff --git a/Lib/various/odehybrid/interpd.m b/Lib/various/odehybrid/interpd.m
new file mode 100644
index 0000000..e36056f
--- /dev/null
+++ b/Lib/various/odehybrid/interpd.m
@@ -0,0 +1,142 @@
+function xi = interpd(t, x, ti, mode, varargin)
+
+% interpd
+%
+% An interpolation function for time series which may have multiple values
+% at a single timestep (e.g., discrete updates). It works like MATLAB's 
+% built-in interp1.
+% 
+% xi = interpd(t, x, ti);
+% xi = interpd(t, x, ti, mode);
+% xi = interpd(t, x, ti, mode, ...);
+%
+% Inputs:
+% 
+% t     Times (n-by-1)
+% x     States (n-by-m)
+% ti    Output times (p-by-1)
+% mode  '-' for left-most values, '+' for right-most values (default)
+% ...   Any additional arguments to be based along to interp1 (e.g., 
+%       'nearest').
+%
+% Outputs:
+%
+% xi  States corresponding to output times (p-by-m)
+% 
+% Example:
+% 
+% % Create some times and states. Note that there are two states at t=3.
+% t = [2.76, 2.91, 3,   3,   3.12].';
+% x = [0.2,  0.3,  0.4, 1.1, 1.2].';
+% 
+% % Create the desired output times.
+% ti = (2.8:0.1:3.1).';
+%
+% % Interpolate (t, x) linearly at ti, keeping the right-most values.
+% xi = interpd(t, x, ti).'
+% 
+% % Interpolate (t, x) linearly at ti, keeping the left-most values.
+% xi = interpd(t, x, ti, '-').'
+% 
+% % Interpolate (t, x) using 'nearest' at ti, keeping the left-most values.
+% xi = interpd(t, x, ti, '-', 'nearest').'
+% 
+% See also: examples_odehybrid.
+%
+% Online doc: http://www.anuncommonlab.com/doc/odehybrid/interpd.html
+%
+% Copyright 2014 An Uncommon Lab
+    
+    % Check that t and ti are columns.
+    if ~any(size(t) == 1)
+        error('interpd:InvalidInputs', 'Input t should be a column.');
+    else
+        t = t(:);
+    end
+    if ~any(size(ti) == 1)
+        error('interpd:InvalidInputs', 'Input ti should be a column.');
+    else
+        ti = ti(:);
+    end
+    
+    % Check that the rows of x match the rows of t.
+    if size(x, 1) ~= length(t) && size(x, 2) == length(t)
+        x = x.';
+    end
+
+    % Set the default mode.
+    if nargin < 4 || isempty(mode)
+        mode = '+';
+    end
+
+    % Create the output.
+    xi = zeros(length(ti), size(x, 2));
+    
+    % Find where there are doubled steps (t(doubled(k)) and t(doubled(k)+1)
+    % are the same.
+    doubled = find(t(1:end-1) == t(2:end));
+    
+    % If nothing is doubled, just pass along to interp1.
+    if isempty(doubled)
+        xi = interp1(t, x, ti, varargin{:});
+        return;
+    end
+    
+    % We will break the interpolation into separate pieces -- those between
+    % the doubled (or tripled or otherwise duplicated) time steps. Then
+    % we'll interpolate that span. We'll keep the edges according to
+    % whether we're using - or + mode.
+    
+    % Make sure we get the first and last spans.
+    if doubled(end) ~= length(t)
+        doubled(end+1, 1) = length(t);
+    end
+    
+    % Start with the initial point.
+    if strcmp(mode, '-') && doubled(1) ~= 1
+        xi(1, :) = interp1(t(1:2), x(1:2, :), ti(1), varargin{:});
+    else
+        % We may overwrite this for +.
+        xi(1, :) = x(1, :);
+    end
+    
+    % For each doubled index...
+    k_last = 1;
+    for k = doubled.'
+        
+        % Get the span from the last index to this one.
+        tk  = t(k_last:k);
+        xk  = x(k_last:k, :);
+        
+        % Get the range for the requested outputs.
+        out_indices = ti >= t(k_last) & ti <= t(k);
+        
+        % If we're using - mode, drop the first index; we don't want to
+        % overwrite the value from the end of the last span with the value
+        % from the beginning of this one.
+        if strcmp(mode, '-')
+            out_indices(find(out_indices, 1, 'first')) = false;
+        end
+        
+        % If there are any outputs in this span...
+        if any(out_indices)
+            
+            % Select the subset of output times.
+            tik = ti(out_indices);
+
+            % If there's only one point, don't interp. Otherwise, use
+            % interp1.
+            if k - k_last == 0
+                xi(out_indices, :) = xk;
+            else
+                xi(out_indices, :) = interp1(tk, xk, tik, varargin{:});
+            end
+            
+        end
+        
+        % Get ready to move to the next span.
+        k_last = k + 1;
+        
+    end
+    
+end
diff --git a/Lib/various/odehybrid/odehybrid.m b/Lib/various/odehybrid/odehybrid.m
new file mode 100644
index 0000000..94d1c5b
--- /dev/null
+++ b/Lib/various/odehybrid/odehybrid.m
@@ -0,0 +1,647 @@
+function varargout = odehybrid(solver, ode, de, dt, ts, yc0, yd0, varargin)
+
+% odehybrid
+% 
+% Hybrid continuous and discrete propagation.
+%
+% This function propagates an ordinary differential equation along with a
+% discrete update equation in a manner similar to MATLAB's ode45 function
+% (which only propagates an ordinary differential equation). This is useful
+% for implementing discrete-time controllers or simulating processes that
+% are updated at discrete intervals.
+% 
+% A large number of examples can be found in examples_odehybrid or by
+% entering the following at the command line:
+%
+%   home = fileparts(which('examples_odehybrid'));
+%   web(fullfile(home, 'html', 'examples_odehybrid.html'));
+%
+% Interfaces:
+% 
+% [t, yc, td, yd] = odehybrid(solver, ode, ...
+%                             de, dt, ...
+%                             ts, ...
+%                             yc0, yd0);
+% [t, yc1..m, td, yd1..n] = odehybrid(solver, ode, ...
+%                                     de, dt, ...
+%                                     ts, ...
+%                                     {yc1, yc2..m}, {yd1, yd2..n});
+% [t, ..., td, ..., te, yc1..m, yd1..n, ie] = odehybrid(solver, ode, ...
+%                                                       de, dt, ...
+%                                                       ts, ...
+%                                                       yc0, yd0);
+% [...] = odehybrid(solver, ode, ...
+%                   {de1, de2, ...}, [dt1, dt2, ...], ...
+%                   ts, ...
+%                   yc0, yd0);
+% [...] = odehybrid(..., [options], [log]);
+% sol = odehybrid(...);
+%
+% Inputs:
+%
+% solver   Continuous-time propagator to use, e.g. @ode45
+% ode      Ordinary differential equation to use with solver. The interface
+%          should be fun(t, xc1, xc2, ..., xcm, xd1, xd2, ..., xdn) where
+%          xc1, xc2, ..., xcm are the continuous states and xd1, xd2, ..., 
+%          xdn are the discrete states. It should return the derivatives of
+%          the continuous states (m outputs).
+% de       Discrete update equation(s) (either a function handle or cell
+%          array of function handles) with the same inputs as ode but
+%          outputing the updated continuous and discrete states (n+m
+%          outputs).
+% dt       Time step(s) of discrete update equation(s). If de is a cell
+%          array of multiple functions, this should be an array of the same
+%          size.
+% ts       Time span, [t_start, t_end]
+% yc0      Cell array of initial continuous states
+% yd0      Cell array of initial discrete states
+% options  (Optional) options structure from odeset
+% log      (Optional) TimeSeriesLogger for logging in the ode and de. If a
+%          log is passed in, both ode and de *must* be able to accomodate
+%          having a log input or not as the final argument. E.g., |ode|
+%          will be called as: ode(t, xc1, ..., xd1, ..., xdn) and
+%          ode(t, xc1, ..., xd1, ..., xdn, log).
+%
+% Outputs:
+% 
+% t        Times corresponding to continuous states (nc-by-1)
+% yc1..m   Continuous state outputs (m outputs, each nc-by-1)
+% td       Times corresponding to discrete state updates (nd-by-1)
+% yd1..n   Discrete state outputs (n outputs, each nd-by-1)
+% te       Times corresponding to events (ne-by-1)
+% yce1..m  Continuous states at events (m outputs, each ne-by-1)
+% yde1..n  Discrete states at events (n outputs, each ne-by-1)
+% ie       Index of triggering event. See documentation in odeset for more 
+%          on events.
+% sol      If a single output is requested, it will be a structure with
+%          fields for each of the individual outputs. The various
+%          continuous states will be grouped in 'yc', the discrete into
+%          'yd', the continuous states at events into 'yce', and the
+%          discrete states at events into 'yde'.
+%
+% Example:
+% 
+% This is a quick example of simulating an unstable continuous system with
+% a stabilizing discrete-time controller.
+% 
+% ode = @(t, x, u) [0 1; 2 0] * x + [0; 1] * u;  % Differential equation
+% de  = @(t, x, u) deal(x, -[8 4] * x);          % Discrete update equation
+% dt  = 0.1;                                     % Discrete eq. time step
+% ts  = [0 5];                                   % From 0 to 5s
+% x0  = [1; 0];                                  % Initial continuous state
+% u0  = 0;                                       % Initial discrete state
+% [t, x, tu, u] = odehybrid(@rkadapt, ode, de, dt, ts, x0, u0); % Simulate!
+% plot(t, x, tu, u, '.'); xlabel('Time');                       % Plot 'em.
+% legend('x_1', 'x_2', 'u');                                    % Label 'em.
+%
+% See also: examples_odehybrid, ode45, rkadapt, rkfixed.
+%
+% Online doc: http://www.anuncommonlab.com/doc/odehybrid/odehybrid.html
+%
+% Copyright 2014 An Uncommon Lab
+
+    % Check inputs
+    if nargin < 7
+        error('odehybrid:TooFewArgs', 'Too few input arguments.');
+    elseif length(ts) ~= 2
+        error('odehybrid:InvalidArguments', ...
+              ['Propagation window should specify both start and stop '...
+               'times and only start and stop times.']);
+    elseif nargin >= 8 && ~isempty(varargin{1}) && ~isstruct(varargin{1})
+        error('odehybrid:InvalidArguments', ...
+              'Expected odeset for argument 8.');
+    elseif nargin >= 9 && ~isa(varargin{2}, 'TimeSeriesLogger')
+        error('odehybrid:InvalidArguments', ...
+              'Expected TimeSeriesLogger for argument 9.');
+    end
+
+    % We have to intercept the output_fcn or it will get called with 'init'
+    % and 'done' for every short step between the discrete steps. That's
+    % probably undesirable.
+    if nargin >= 8 && ~isempty(varargin{1})
+        if ~isempty(varargin{1}.OutputFcn)
+            f = varargin{1}.OutputFcn;
+            varargin{1}.OutputFcn = @(varargin) ...
+                       output_fcn(f, varargin{:}, yc0);
+        end
+        if ~isempty(varargin{1}.Events)
+            f = varargin{1}.Events;
+            varargin{1}.Events = @(varargin) ...
+                       event_fcn(f, varargin{:}, yc0);
+        end
+    end
+    
+	% See if we're passing in separated states (the "full" version).
+    if ~isnumeric(yc0) || ~isnumeric(yd0)
+
+        % Get the full inputs.
+        [varargout{1:nargout}] = odehybridfull(solver, ode, de, dt, ...
+                                               ts, yc0, yd0, varargin{:});
+
+	% Otherwise, just pass everything on directly to the odehybridcore.
+    else
+        [varargout{1:nargout}] = odehybridcore(solver, ode, de, dt, ...
+                                               ts, yc0, yd0, ...
+                                               varargin{:});
+    end
+
+end
+
+% odehybridcore(solver, ode, de, dt, ts, yc0, yd0, [options], [log])
+function [t, yc, td, yd, te, yce, yde, ie] = odehybridcore(solver, ...
+                                                           ode, ...
+                                                           de, dt, ...
+                                                           ts, yc0, yd0,...
+                                                           varargin)
+    
+    % Ignore growth in loops. We're just going to have this problem.
+    %#ok<*AGROW>
+    
+    % For multiple sample rates, we'll expect the discrete updates to be
+    % stored in a cell array. If the user is just passing in a single
+    % discrete update, it might not be in a cell array. Put it into one.
+    if ~iscell(de)
+        de = {de};
+    end
+    
+    % Make sure dt is a row vector.
+    if size(dt, 1) > 1
+        dt = dt.';
+    end
+    
+    % Set the solver's options if the user provided them.
+    if nargin >= 8 && ~isempty(varargin{1})
+        options = varargin{1};
+    else
+        options = odeset();
+    end
+
+    % Check for the things we don't support.
+    if ~isempty(options.Vectorized)
+        error('odehybrid:options', ...
+              'odehybrid doesn''t support vectorized functions.');
+    elseif ~isempty(options.OutputSel)
+        error('odehybrid:options', ...
+              ['odehybrid doesn''t support the OutputSel option ' ...
+               'because states need not be vectors.']);
+    end
+    
+    % Figure out the time steps. First, calculate the resolution with which 
+    % we will be able to differentiate steps. Then, make arrays for each 
+    % separate time step.
+    epsilon = 2 * max((diff(ts) ./ dt) .* eps(dt) + eps(ts(2)));
+    tds = cell(1, length(dt));
+    for k = 1:length(dt)
+        tds{k} = ts(1):dt(k):ts(end);
+    end
+    
+    % Sort the necessary discrete times into a single list and remove
+    % doubled steps. We now have a list of all times at which to break for
+    % a discrete step of one or more discrete update functions.
+    td = sort([tds{:}]);
+    td([false diff(td) < epsilon]) = [];
+    td = td(:);
+    
+    % We're going to overwrite the output_fcn, so store the original.
+    orig_outputfcn = options.OutputFcn;
+    
+    % We're going to overwrite the output_fcn, so store the original.
+    orig_eventfcn = options.Events;
+    
+    % If the user passed in a log, use it.
+    add_logging = nargin >= 9 && ~isempty(varargin{2});
+    if add_logging
+        
+        % Add logging to the output (preserving the user's outputfcn if
+        % it's provided).
+        log = varargin{2};
+        for k = 1:length(dt)
+            de{k} = @(t, yc, yd) de{k}(t, yc, yd, log);
+        end
+        
+    else
+        log = [];
+    end
+    
+    % Set the maximum time step to be the smaller of what's already
+    % specified or dt.
+    if isempty(options.MaxStep)
+        options.MaxStep = min(dt);
+    else
+        options.MaxStep = min(options.MaxStep, min(dt));
+    end
+    
+    % Set the initial step.
+    if isempty(options.InitialStep)
+        options.InitialStep = min(0.1 * min(dt), 0.01 * diff(ts));
+    end
+    
+    % Initialize outputs.
+    t  = ts(1); yc = yc0.';
+    te = []; yce = []; ie = []; % Events
+    if iscell(yd0)
+        yd = cell(length(td), numel(yd0));
+        [yd{1, :}] = yd0{:};
+        yde = {};
+    else
+        yd = [yd0.'; zeros(length(td)-1, length(yd0))];
+        yde = [];
+    end
+    
+    % Call the output function if there is one.
+    if ~isempty(orig_outputfcn)
+        orig_outputfcn(ts, yc0, yd0, 'init');
+    end
+    
+    % Loop through all k, updating from k to k+1.
+    counts = zeros(size(dt));
+    for k = 1:length(td)
+        
+        %%%%%%%%%%%%%%%%%%%%
+        % Discrete Updates %
+        %%%%%%%%%%%%%%%%%%%%
+        
+        if k <= length(td)
+
+            % Find out what should discrete functions should be called at 
+            % this time.
+            to_tick = find(abs(counts .* dt + ts(1) - td(k)) <= epsilon);
+
+            % Call the relevant discrete updates.
+            for z = to_tick
+                [yc0, yd0] = de{z}(td(k), yc0(:), yd0, varargin);
+            end
+
+            % We can now store the updated state for k. 
+            if iscell(yd0)
+                [yd{k, :}] = yd0{:};
+            else
+                yd(k, :) = yd0.';
+            end
+
+            % Tick them.
+            counts(to_tick) = counts(to_tick) + 1;
+    
+            % Call the output function with the udpated discrete states.
+            if    ~isempty(orig_outputfcn) ...
+               && orig_outputfcn(td(k), yc0, yd0, '');
+                td = td(1:k);
+                if iscell(yd)
+                    yd = yd(1:k);
+                else
+                    yd = yd(1:k, :);
+                end
+                break;
+            end
+            
+        end
+        
+        %%%%%%%%%%%%%%%%%%%%%%
+        % Continuous Updates %
+        %%%%%%%%%%%%%%%%%%%%%%
+        
+        % If we're at the end of the discrete steps, see if there's still a
+        % little more left to simulation in continuous time. If so, create
+        % the correct time span. If there's no continuous time left, just
+        % break (we're done). And if we're not at the end of the discrete
+        % steps, just create the right time window.
+        if k == length(td)
+            if ts(2) - td(k) > epsilon
+                tsk = [td(k), ts(2)];
+            else
+                break;
+            end
+        else
+            tsk = [td(k), td(k+1)];
+        end
+        
+        % Make the output function with the new discrete states.
+        if add_logging || ~isempty(options.OutputFcn)
+            options.OutputFcn = @(t, y, flag) log_frame(t, y, flag, ...
+                                                        ode, yd0, log, ...
+                                                        orig_outputfcn);
+        end
+        if ~isempty(options.Events)
+            options.Events = @(t, y) orig_eventfcn(t, y, yd0);
+        end
+        
+        % If there are events, output one way. If not, output the basic
+        % way.
+        if isempty(options.Events)
+            [tk, yck] = solver(@(t, y) ode(t, y, yd0), ...
+                               tsk, ...
+                               yc0, ...
+                               options);
+        else
+            [tk, yck, tek, yek, iek] = solver(@(t, y) ode(t, y, yd0), ...
+                                              tsk, ...
+                                              yc0, ...
+                                              options);
+            if nargout == 1 || nargout >= 3, te  = [te; tek];  end;
+            if nargout == 1 || nargout >= 4, yce = [yce; yek]; end;
+            if nargout == 1 || nargout >= 6, ie  = [ie; iek];  end;
+            if nargout == 1 || nargout >= 5
+                for z = 1:length(tek)
+                    if iscell(yd0)
+                        [yde{end+1, 1:numel(yd0)}] = yd0{:};
+                    else
+                        yde = [yde; yd0(:).'];
+                    end
+                end
+            end;
+        end
+        
+        % Add the outputs to the list.
+        t  = [t;  tk];
+        yc = [yc; yck];
+        
+        % See if the function terminated early.
+        if abs(tk(end) - tsk(end)) > 2*eps(tsk(end))
+            td = td(1:k);
+            if iscell(yd)
+                yd = yd(1:k);
+            else
+                yd = yd(1:k, :);
+            end
+            break;
+        end
+        
+        % Set the initial step for next time to be the largest step we took
+        % during this iteration.
+        options.InitialStep = max(diff(tk));
+        
+        % Get ready for the next step.
+        yc0 = yc(end, :)';
+        
+    end
+    
+    % Call the output function if there is one.
+    if ~isempty(orig_outputfcn)
+        orig_outputfcn(ts, yc0, yd0, 'done');
+    end
+
+    % If there's only one output, use the structure format.
+    if nargout == 1
+        sol = struct('t',   t, ...
+                     'yc',  yc, ...
+                     'td',  td, ...
+                     'yd',  yd, ...
+                     'te',  te, ...
+                     'yce', yce, ...
+                     'yde', yde, ...
+                     'ie',  ie);
+        t = sol;
+    end
+    
+end
+
+% We customize the output function to ignore 'init' and 'done'.
+function status = output_fcn(f, t, ycv, yd, flag, yc0)
+
+    % We're going to rely on cell arrays to pass these to the original
+    % output function, so make sure they're cell arrays.
+    if ~iscell(yc0), yc0 = {yc0}; end;
+    if ~iscell(yd),  yd  = {yd};  end;
+    
+    switch flag
+        
+        % For 'done', all of the states should be empty.
+        case 'done'
+
+            % Pull out the continuous state first (this is how we store it).
+            yc = vector_to_state(ycv, yc0);
+
+            for k = 1:length(yc)
+                yc{k} = [];
+            end
+            for k = 1:length(yd)
+                yd{k} = [];
+            end
+            status = f([], yc{:}, yd{:}, flag);
+
+        % For init, we pass the time span along with the initial state.
+        case 'init'
+
+            % Pull out the continuous state first.
+            yc = vector_to_state(ycv, yc0);
+            status = f(t, yc{:}, yd{:}, flag);
+    
+        % Otherwise, pass along the states and the flag.
+        otherwise
+
+            for k = 1:length(t)
+
+                % Pull out the continuous state first.
+                yc = vector_to_state(ycv(:, k), yc0);
+                status = f(t(k), yc{:}, yd{:}, flag);
+
+            end
+            
+    end
+        
+end
+
+% We customize the output function to ignore 'init' and 'done'.
+function varargout = event_fcn(f, t, ycv, yd, yc0)
+
+    % We're going to rely on cell arrays to pass these to the original
+    % output function, so make sure they're cell arrays.
+    if ~iscell(yc0), yc0 = {yc0}; end;
+    if ~iscell(yd),  yd  = {yd};  end;
+    
+    % Pull out the continuous state first.
+    yc = vector_to_state(ycv, yc0);
+    [varargout{1:nargout}] = f(t, yc{:}, yd{:});
+
+end
+
+% Run the ODE again, this time using the log.
+function status = log_frame(t, y, flag, ode, yd, log, f)
+
+    % We never tell integration to stop (but the user's function can
+    % override this).
+    status = 0;
+
+    % Only call the ODE with the logger on "normal" steps (steps with 
+    % neither 'init' nor 'done' flags).
+    if isempty(flag)
+
+        % If there's a TimeSeriesLogger, use it.
+        if ~isempty(log)
+            for k = 1:length(t)
+                ode(t(k), y(:, k), yd, log);
+            end
+        end
+
+        % Call the user's OutputFcn if provided.
+        if ~isempty(f)
+            status = f(t, y, yd, flag);
+        end
+        
+    end
+
+end
+
+% Run the continuous-discrete-input version of odehybrid.
+function varargout = odehybridfull(solver, ode, de, dt, ...
+                                   ts, yc0, yd0, varargin)
+
+    % Let the user pass in cells or anything else, but always make sure we
+    % work with cells. This simplifies life when we have to dynamically
+    % pass these into functions.
+    if ~iscell(yc0), yc0 = {yc0}; end;
+    if ~iscell(yd0), yd0 = {yd0}; end;
+    if ~iscell(de),  de  = {de};  end;
+    
+    % Create the initial continuous state vector.
+    yc0v = state_to_vector(yc0);
+
+    % Create a function to expand the vector into the state, pass the state
+    % to the ODE, and turn the result back into a vector.
+    ode_v = @(t,yc,yd,varargin) run_ode(ode, t, yc, yd, yc0, varargin{:});
+    for k = 1:length(dt)
+        de_v{k} = @(t, yc, yd, varargin) run_de(de{k}, t, yc, yd, yc0, ...
+                                                varargin{:});
+    end
+
+    % Determine what outputs we need.
+    if nargout == 1                                     % Structure
+        n_outputs = 8;
+    elseif nargout <= 1 + numel(yc0)                    % Continuous
+        n_outputs = 2;
+    elseif nargout <=   1 + numel(yc0) ...              % Continuous
+                      + 1 + numel(yd0)                  % Discrete
+        n_outputs = 4;
+    elseif nargout <=   1 + numel(yc0) ...              % Continuous
+                      + 1 + numel(yd0) ...              % Discrete
+                      + 1 + numel(yc0) + numel(yd0) + 1 % Events
+        n_outputs = 8;
+    else
+        error('Too many outputs are requested from odoehybrid.');
+    end
+    
+    % Propagate the vector versions of the ODE and DE.
+    outputs = cell(1, n_outputs);
+    [outputs{:}] = odehybridcore(solver, ...
+                                 ode_v, de_v, dt, ts, yc0v, yd0, ...
+                                 varargin{:});
+
+	% For states at events, convert to the appropriate type from the big
+	% cell array of stored values.
+    if n_outputs >= 8
+        
+        % For the continuous part...
+        continuous_states = cell(1, numel(yc0));
+        [continuous_states{:}] = vectors_to_states(outputs{6}, yc0{:});
+
+        % Make the output.
+        outputs = [outputs(1:5), ...
+                   continuous_states, ...
+                   output_discrete_states(outputs{7}, yd0), ...
+                   outputs(8:end)];
+        
+    end
+
+	% For discrete states, convert to the appropriate type from the big
+	% cell array of stored values.
+    if n_outputs >= 4
+        outputs = [outputs(1:3), ...
+                   output_discrete_states(outputs{4}, yd0), ...
+                   outputs(5:end)];
+    end
+    
+    % For continuous state, convert back to original types from the state
+    % vectors.
+    if n_outputs >= 2
+        continuous_states = cell(1, numel(yc0));
+        [continuous_states{:}] = vectors_to_states(outputs{2}, yc0{:});
+        outputs = [outputs(1), ...
+                   continuous_states, ...
+                   outputs(3:end)];
+    end
+    
+	% Return separately the states as cell arrays.
+    varargout = outputs;
+
+    % If there's only one output, use the structure format.
+    if nargout == 1
+        
+        sol.t   = outputs{1};
+        sol.yc  = outputs(1+(1:numel(yc0)));
+        sol.td  = outputs{1+numel(yc0)+1};
+        sol.yd  = outputs(1+numel(yc0)+1+(1:numel(yd0)));
+        sol.te  = outputs{1+numel(yc0)+1+numel(yd0)+1};
+        sol.yce = outputs(1+numel(yc0)+1+numel(yd0)+1+(1:numel(yc0)));
+        sol.yde = outputs(  1+numel(yc0)+1+numel(yd0)+1+numel(yc0) ...
+                          + (1:numel(yd0)));
+        sol.ie  = outputs{end};
+        varargout{1} = sol;
+    end
+
+end
+
+% Convert a state vector to the appropriate state, run the ODE, and
+% turn the result back into a vector.
+function dyvdt = run_ode(ode, t, ycv, yd, yc0, varargin)
+
+    % Pull out the continuous state first (this is how we store it).
+    yc = vector_to_state(ycv, yc0);
+    
+    % Get the state differences.
+    [state_difference{1:numel(yc0)}] = ode(t, yc{:}, yd{:}, varargin{:});
+    
+    % Convert the derivative to a vector.
+    dyvdt = state_to_vector(state_difference);
+    
+end
+
+% Convert a state vector to the appropriate state, run the ODE, and
+% turn the result back into a vector.
+function [ycv, yd] = run_de(de, t, ycv, yd, yc0, varargin)
+
+    % Pull out the continuous state first (this is how we store it).
+    yc = vector_to_state(ycv, yc0);
+
+    % Run the update.
+    [yc{:}, yd{:}] = de(t, yc{:}, yd{:}, varargin{:});
+    
+    % Convert to vectors.
+    ycv = state_to_vector(yc);
+    
+end
+
+% Turn an n-by-m cell array of discrete states into a 1-by-m cell array of
+% discrete state lists each with n entries.
+function discrete_states = output_discrete_states(states, yd0)
+
+    % Make space for the individual outputs.
+    discrete_states = cell(1, numel(yd0));
+
+    % For each output...
+    for k = 1:numel(yd0)
+
+        % Convert to a matrix with rows containing vectors.
+        if isnumeric(yd0{k}) || ischar(yd0{k})
+
+            dim = find([size(yd0{k}), 1] == 1, 1, 'first');
+            if dim == 2
+                discrete_states{k} = cat(dim, states{:, k}).';
+            else
+                discrete_states{k} = cat(dim, states{:, k});
+            end
+
+        % Convert to an array of structs.
+        elseif isstruct(yd0{k}) && length(yd0{k}) == 1
+
+            discrete_states{k} = cellfun(@(v) v, states(:, k));
+
+        % Give up and just use the cell array of states.
+        else
+
+            discrete_states{k} = states(:, k);
+
+        end
+
+    end
+        
+end
diff --git a/Lib/various/odehybrid/rk4.m b/Lib/various/odehybrid/rk4.m
new file mode 100644
index 0000000..927abfc
--- /dev/null
+++ b/Lib/various/odehybrid/rk4.m
@@ -0,0 +1,111 @@
+function [t, x] = rk4(ode, ts, x0, dt)
+
+% rk4
+% 
+% Runge-Kutta 4th order integration method.
+% 
+% Implements numerical propagation of an ordinary differential equation
+% from some initial value over the desired range. This function is similar
+% to MATLAB's variable-step ODE propagators (e.g., ode45), but uses a
+% fixed step method. This is useful either when one knows an appropriate 
+% step size or when a process is interrupted frequently (ode45 and the
+% similar functions in MATLAB will always make at least a certain number of
+% steps between ts(1) and ts(2), which may be very many more than are
+% necessary).
+%
+% [t, x] = rk4(ode, ts, x0, dt);
+% [t, x] = rk4(ode, ts, x0, options);
+%
+% Inputs:
+% 
+% ode     Ordinary differential equation function
+% ts      Time span, [t_start, t_end]
+% x0      Initial state (column vector)
+% dt      Time step
+% options Alternately, one can specify an options structure instead of dt
+%         so that this function is compatible with ode45 and its ilk. The
+%         only valid fields are MaxStep (the time step) and OutputFcn
+%
+% Outputs:
+% 
+% t      Time history
+% x      State history, with each row containing the state corresponding to
+%        the time in the same row of t.
+%
+% Example:
+% 
+% % Simulate an undamped harmonic oscillator for 10s with a 0.1s time
+% % step, starting from an initial state of [1; 0].
+% ode = @(t, x) [-x(2); x(1)];
+% [t, x] = rk4(ode, [0 10], [1; 0], 0.1);
+% plot(t, x);
+%
+% See also: odehybrid, ode45, odeset, rkfixed, rkadapt.
+%
+% Online doc: http://www.anuncommonlab.com/doc/odehybrid/rk4.html
+%
+% Copyright 2014 An Uncommon Lab
+
+    % Allow an alternate input syntax to be similar with ode45.
+    if isstruct(dt)
+        options = dt;
+        dt = options.MaxStep;
+        if isempty(dt)
+            error('Specify the time step with the MaxStep option.');
+        end
+    else
+        options.OutputFcn = [];
+    end
+
+    % Time history, making sure the end time is explicity represented.
+    t  = (ts(1):dt:ts(end)).';
+    if t(end) ~= ts(end)
+        t(end+1, 1) = ts(end);
+    end
+    
+    ns = length(t);                   % Number of samples
+    nx = numel(x0);                   % Number of states
+    x  = [x0(:).'; zeros(ns-1, nx)];  % State history
+    xk = x0(:);                       % Current state
+    hdt = 0.5 * dt;                   % Half of the time step
+    
+    % If the user provided an OutputFcn, call it.
+    if ~isempty(options.OutputFcn)
+        options.OutputFcn(ts, x0, 'init');
+    end
+    
+    % Propagate from each k to k+1.
+    for k = 1:ns-1
+        
+        % The last sample may be cut short to have the correct end time.
+        if k == ns-1
+            dt = t(k+1) - t(k);
+            hdt = 0.5 * dt;
+        end
+        
+        % From k to k+1
+        tk = t(k);                                        % Current time
+        k1 = ode(tk,       xk);                           % RK derivatives
+        k2 = ode(tk + hdt, xk + hdt * k1);                % ...
+        k3 = ode(tk + hdt, xk + hdt * k2);
+        k4 = ode(tk +  dt, xk +  dt * k3);
+        xk = xk + (k1 + 2 * k2 + 2 * k3 + k4) * (dt / 6); % Updated state
+        x(k+1, :) = xk(:).';                              % Store.
+        
+        % If the user provided an OutputFcn, call it.
+        if ~isempty(options.OutputFcn)
+            if options.OutputFcn(t(k+1), xk, '');
+                t = t(1:k+1);
+                x = x(1:k+1, :);
+                break;
+            end
+        end
+    
+    end
+
+    % If the user provided an OutputFcn, call it.
+    if ~isempty(options.OutputFcn)
+        options.OutputFcn([], [], 'done');
+    end
+    
+end
diff --git a/Lib/various/odehybrid/rkadapt.m b/Lib/various/odehybrid/rkadapt.m
new file mode 100644
index 0000000..912ed6d
--- /dev/null
+++ b/Lib/various/odehybrid/rkadapt.m
@@ -0,0 +1,261 @@
+function [t, x] = rkadapt(ode, ts, x0, options, a, b, c, p)
+
+% rkadapt
+% 
+% Runge-Kutta adaptive-step integration using the specified weights, nodes,
+% and Runge-Kutta matrix (or Dormand-Prince 5(4) by default).
+% 
+% Implements numerical propagation of an ordinary differential equation
+% from some initial value over the desired range. This function is similar
+% to MATLAB's variable-step ODE propagators (e.g., ode45), but is 
+% generalized to any Butcher tableau. Unlike ode45, however, there's no
+% inherent minimum number of steps that this function will take; if it can
+% finish integration with a single step, it will, whereas ode45 will always
+% take at least a certain number of steps, no matter how small the time
+% interval and how smooth the dynamics. Therefore, this function is
+% commonly more useful with odehybrid than ode45.
+%
+% This function is generic for all adaptive-step Runge-Kutta methods. That
+% is, any adaptive-step Runge-Kutta propagator can be created by passing
+% the weightes, nodes, and Runge-Kutta matrix (together, the Butcher 
+% tableau) into this function. It will correctly take advantage of the 
+% first-same-as-last property, eliminating one function evaluation per 
+% step when the table has this property. See the examples below.
+%
+% [t, x] = rk4adapt(ode, ts, x0);
+% [t, x] = rk4adapt(ode, ts, x0, options);
+% [t, x] = rk4adapt(ode, ts, x0, options, a, b, c, p);
+%
+% Inputs:
+% 
+% ode     Ordinary differential equation function (s.t. x_dot = ode(t, x);)
+% ts      Time span, [t_start, t_end]
+% x0      Initial state (column vector)
+% options Options structure from odeset. This function uses the
+%         InitialStep, MaxStep, OutputFcn, RelTol, and AbsTol fields only.
+% a       Runge-Kutta matrix (s-by-s)
+% b       Weights (2-by-s), the top row containing the weights for the 
+%         state update
+% c       Nodes (s-by-1)
+% p       Lowest order method represented by b (e.g., for
+%         Runge-Kutta-Fehlberg, this is 4).
+%
+% Outputs:
+% 
+% t      Time history
+% x      State history, with each row containing the state corresponding to
+%        the time in the same row of t.
+%
+% Example:
+% 
+% % Simulate an undamped harmonic oscillator for 10s starting from an 
+% % initial state of [1; 0] using the default a, b, c, and p
+% % (Dormand-Prince 5(4)).
+% [t, x] = rkadapt(@(t,x) [-x(2); x(1)], [0 10], [1; 0]);
+% plot(t, x);
+% 
+% % Add options.
+% options = odeset('MaxStep', 0.1, 'InitialStep', 0.05);
+% [t, x] = rkadapt(@(t,x) [-x(2); x(1)], [0 10], [1; 0], options);
+% plot(t, x);
+% 
+% % Now simulate with Bogacki-Shampine 3(2).
+% a = [  0   0   0 0; ...
+%      1/2   0   0 0; ...
+%        0 3/4   0 0; ...
+%      2/9 1/3 4/9 0];
+% b = [ 2/9 1/3 4/9   0; ... % For the update
+%      7/24 1/4 1/3 1/8];    % For the error prediction
+% c = [0 1/2 3/4 1];
+% p = 2;                     % The lower of the two orders
+% [t, x] = rkadapt(@(t,x) [-x(2); x(1)], [0 10], [1; 0], [], a, b, c, p);
+% plot(t, x);
+%
+% See "Runge-Kutta methods" on Wikipedia for discussion of the Butcher
+% tableau (a, b, and c).
+%
+% Reference: 
+%
+% Dormand, J. R., and P. J. Prince. "A family of embedded Ruge-Kutta
+% formulae." _Journal of Computational and Applied Mathematics_ 6.1 (1980):
+% 19-26. Web. 22 Apr. 2014.
+%
+% See also: odehybrid, ode45, odeset, rkfixed.
+%
+% Online doc: http://www.anuncommonlab.com/doc/odehybrid/rkadapt.html
+%
+% Copyright 2014 An Uncommon Lab
+
+    % Check the arguments.
+    if nargin < 3
+        error('rkadapt:IncorrectArguments', ...
+              'Incorrect number of arguments.');
+    elseif nargin >= 5 && nargin < 8
+        error('rkadapt:IncorrectArguments', ...
+              ['If any of a, b, c, and p are specified, all must ' ...
+               'be specified.']);
+    end
+    
+    % If there's no options structure, make one.
+    if nargin == 3 || (nargin >= 4 && isempty(options))
+        options = odeset();
+    end
+    
+    % If there's no table, default to Dormand-Prince 5(4).
+    if nargin < 8
+        [a, b, c, p] = dptable();
+    end
+
+    % Unpack what we care about from the options.
+    rel_tol = default(options.RelTol, 1e-3);
+    abs_tol = default(options.AbsTol, 1e-6);
+    dt_max  = default(options.MaxStep, inf);
+    dt      = min(default(options.InitialStep, 1e-3), dt_max);
+    
+    % See if this table has the first-same-as-last property.
+    fsal = all(a(end, :) == b(1, :));
+    
+    ex = 1/(p+1);                     % Time step correction exponent
+    dt_min = 100 * eps(max(abs(ts))); % Minimum time step
+
+    t   = ts(1);       % Time history
+    x   = x0(:).';     % State history
+    tk  = ts(1);       % Current time
+    xk  = x0(:);       % Current state
+    xkp = xk;          % Proposed state update
+    
+    nx = numel(x0);    % Number of states
+    s  = size(b, 2);   % Length of weights
+    d  = zeros(nx, s); % Matrix of derivatives
+    db = b(2, :) - b(1, :);
+    
+    % If the user provided an OutputFcn, call it.
+    if ~isempty(options.OutputFcn)
+        options.OutputFcn(ts, x0, 'init');
+    end
+    
+    % If using a table with FSAL, start the derivative here. 
+    if fsal
+        d(:, 1) = ode(tk, xk);
+    end
+    
+    % Propagate, updating k to k+1 until we're done until t(k) == ts(2).
+    stop = false;
+    while tk < ts(2) - dt_min
+        
+        % Don't step past the end time.
+        if tk + dt > ts(2)
+            dt = ts(2) - tk;
+        end
+        
+        % Evaluate the first bit of the derivatives.
+        if ~fsal
+            d(:, 1) = ode(tk, xk);
+        end
+        
+        % Try to take a step.
+        while true
+            
+            % Calculate derivatives.
+            for z = 2:s
+                dxk = sum(bsxfun(@times, dt*a(z, 1:z-1), d(:, 1:z-1)), 2);
+                d(:, z) = ode(tk + c(z) * dt, xk + dxk);
+            end
+
+            % Update the state and error estimate.
+            tkp = tk + dt;
+            xkp = xk;
+            er  = zeros(nx, 1);
+            for z = 1:s
+                xkp = xkp + (dt * b(1, z)) * d(:, z);
+                er  = er  + (dt * db(z))   * d(:, z);
+            end
+
+            % Create the scaled error. This single number can now be
+            % compared to 1 to see if the worst error satisfies both 
+            % absolute and relative tolerances. The error is acceptable if
+            % it is either less than the relative error or less than the
+            % aboslute error.
+            scaled_error = max(abs(er) ./ (abs_tol + rel_tol * abs(xkp)));
+
+            % If the error is small, update the time step and break to the
+            % next step.
+            if scaled_error <= 1
+                
+                dt = min(0.9 * dt * scaled_error^-ex, min(3*dt, dt_max));
+                break;
+                
+            % If the error is too large, update the time step and continue
+            % the search for an appropriate time step.
+            else
+
+                % If that was already the smallest step we can take, give
+                % up.
+                if dt == dt_min
+                    warning('Could not meet integration tolerance.');
+                    break;
+                end
+                
+                % Cut the time step down faster than we build it up.
+                dt = max(0.7 * dt * scaled_error^-ex, dt_min);
+                
+            end
+            
+        end
+
+        % Accept the update.
+        tk = tkp;
+        xk = xkp;
+        
+        % For first-same-as-last, we can copy over the derivative at k.
+        if fsal
+            d(:, 1) = d(:, end);
+        end
+        
+        % Store.
+        t = [t; tk];      %#ok<AGROW>
+        x = [x; xk(:).']; %#ok<AGROW>
+        
+        % If the user provided an OutputFcn, call it.
+        if ~isempty(options.OutputFcn)
+            stop = options.OutputFcn(tk, xk, '');
+            if stop
+                break;
+            end
+        end
+        
+    end
+
+    % Rectify the ending.
+    if ~stop
+        t(end) = ts(2);
+    end
+    
+    % If the user provided an OutputFcn, call it.
+    if ~isempty(options.OutputFcn)
+        options.OutputFcn([], [], 'done');
+    end
+
+end
+
+% Use the value in f or use v as the default.
+function v = default(f, v)
+    if ~isempty(f)
+        v = f;
+    end
+end
+
+% Dormand-Prince Butcher tableau
+function [a, b, c, p] = dptable()
+    a = [    0           0          0         0         0        0    0;...
+             1/5         0          0         0         0        0    0;...
+             3/40        9/40       0         0         0        0    0;...
+            44/45      -56/15      32/9       0         0        0    0;...
+         19372/6561 -25360/2187 64448/6561 -212/729     0        0    0;...
+          9017/3168 -355/33     46732/5247   49/176 -5103/18656  0    0;...
+            35/384     0          500/1113  125/192 -2187/6784  11/84 0];
+    b = [35/384     0  500/1113  125/192  -2187/6784    11/84   0; ...
+         5179/57600 0 7571/16695 393/640 -92097/339200 187/2100 1/40];
+    c = [0 1/5 3/10 4/5 8/9 1 1];
+    p = 4;
+end
diff --git a/Lib/various/odehybrid/rkfixed.m b/Lib/various/odehybrid/rkfixed.m
new file mode 100644
index 0000000..55cce95
--- /dev/null
+++ b/Lib/various/odehybrid/rkfixed.m
@@ -0,0 +1,153 @@
+function [t, x] = rkfixed(ode, ts, x0, dt, a, b, c)
+
+% rkfixed
+% 
+% Runge-Kutta fixed-step integration using the specified weights, nodes,
+% and Runge-Kutta matrix (or the Runge-Kutta 4th order "3/8" method by
+% default).
+% 
+% Implements numerical propagation of an ordinary differential equation
+% from some initial value over the desired range. This function is similar
+% to MATLAB's variable-step ODE propagators (e.g., ode45), but uses a
+% fixed step method. This is useful either when one knows an appropriate 
+% step size or when a process is interrupted frequently (ode45 and the
+% similar functions in MATLAB will always make at least a certain number of
+% steps between ts(1) and ts(2), which may be very many more than are
+% necessary).
+%
+% This function is generic for all fixed-step Runge-Kutta methods. That is,
+% any fixed-step Runge-Kutta propagator can be created by passing the
+% weightes, nodes, and Runge-Kutta matrix (together, the Butcher tableau) 
+% into this function. See the example below.
+%
+% [t, x] = rkfixed(ode, ts, x0, dt);
+% [t, x] = rkfixed(ode, ts, x0, dt, a, b, c);
+% [t, x] = rkfixed(ode, ts, x0, options);
+% [t, x] = rkfixed(ode, ts, x0, options, a, b, c);
+%
+% Inputs:
+%
+% ode     Ordinary differential equation function
+% ts      Time span, [t_start, t_end]
+% x0      Initial state (column vector)
+% dt      Time step
+% options Alternately, one can specify an options structure instead of dt
+%         so that this function is compatible with ode45 and its ilk. The
+%         only valid fields are MaxStep (the time step) and OutputFcn
+% a       Runge-Kutta matrix
+% b       Weights
+% c       Nodes
+%
+% Outputs:
+%
+% t      Time history
+% x      State history, with each row containing the state corresponding to
+%        the time in the same row of t.
+%
+% Example:
+% 
+% % Simulate an undamped harmonic oscillator for 10s with a 0.1s time
+% % step, starting from an initial state of [1; 0] using RK 4th order
+% % integration (via the Butcher tableau specified by a, b, and c). This
+% % is exactly the same as the rk4 function.
+% a = [0   0   0 0; ...
+%      0.5 0   0 0; ...
+%      0   0.5 0 0; ...
+%      0   0   1 0];
+% b = [1 2 2 1]/6;
+% c = [0 0.5 0.5 1];
+% [t, x] = rkfixed(@(t,x) [-x(2); x(1)], [0 10], [1; 0], 0.1, a, b, c);
+% plot(t, x);
+%
+% See "Runge-Kutta methods" on Wikipedia for discussion of the Butcher
+% tableau (a, b, and c).
+%
+% See also: odehybrid, ode45, odeset, rk4.
+%
+% Online doc: http://www.anuncommonlab.com/doc/odehybrid/rkfixed.html
+%
+% Copyright 2014 An Uncommon Lab
+
+    % Default to Runge and Kutta's 3/8 formulation (4th order).
+    if nargin == 4
+        a = [ 0    0 0 0; ...
+              1/3  0 0 0; ...
+             -1/3  1 0 0; ...
+              1   -1 1 0];
+        b = [1 3 3 1]/8;
+        c = [0 1/3 2/3 1];
+    end
+
+    % Allow an alternate input syntax to be similar with ode45.
+    if isstruct(dt)
+        options = dt;
+        dt = options.MaxStep;
+        if isempty(dt)
+            error('Specify the time step with the MaxStep option.');
+        end
+    else
+        options.OutputFcn = [];
+    end
+
+    % Time history
+    t  = (ts(1):dt:ts(end)).';
+    if t(end) ~= ts(end)
+        t(end+1, 1) = ts(end);
+    end
+    
+    ns = length(t);                   % Number of samples
+    nx = numel(x0);                   % Number of states
+    x  = [x0(:).'; zeros(ns-1, nx)];  % State history
+    xk = x0(:);                       % Current state
+    
+    s = length(b);                    % Length of weights
+    d = zeros(nx, s);                 % Matrix of derivatives
+    
+    % If the user provided an OutputFcn, use it.
+    if ~isempty(options.OutputFcn)
+        options.OutputFcn(ts, x0, 'init');
+    end
+    
+    % Propagate.
+    for k = 1:ns-1
+        
+        % The last sample may be cut short.
+        if k == ns-1
+            dt = t(k+1) - t(k);
+        end
+        
+        % Current time
+        tk = t(k);
+        
+        % Calculate derivatives.
+        d(:, 1) = dt * ode(tk, xk);
+        for z = 2:s
+            dxk = sum(bsxfun(@times, a(z, 1:z-1), d(:, 1:z-1)), 2);
+            d(:, z) = dt * ode(tk + c(z) * dt, xk + dxk);
+        end
+        
+        % Update the state.
+        for z = 1:s
+            xk = xk + b(z) * d(:, z);
+        end
+        
+        % Store.
+        x(k+1, :) = xk(:).';
+        
+        % If the user provided an OutputFcn, use it.
+        if ~isempty(options.OutputFcn)
+            if options.OutputFcn(t(k+1), xk, '');
+                t = t(1:k+1);
+                x = x(1:k+1, :);
+                break;
+            end
+        end
+        
+    end
+
+    % If the user provided an OutputFcn, use it.
+    if ~isempty(options.OutputFcn)
+        options.OutputFcn([], [], 'done');
+    end
+
+end
diff --git a/Lib/various/odehybrid/state_to_vector.m b/Lib/various/odehybrid/state_to_vector.m
new file mode 100644
index 0000000..eba3770
--- /dev/null
+++ b/Lib/various/odehybrid/state_to_vector.m
@@ -0,0 +1,67 @@
+function vector = state_to_vector(state)
+
+% state_to_vector
+%
+% Build a state vector by recursively moving through the elements of a more
+% complex state (matrix, cell array, or struct), saving the numerical 
+% values along the way in a vector. This is used in odehybrid to convert a
+% complex series of states into a vector to be used for continuous state
+% updates. The reverse function is vector_to_state.
+%
+% vector = state_to_vector(state);
+% 
+% Inputs:
+% 
+% state   A numeric, cell array, or struct array type, the contents of
+%         which consist of numeric, cell array, or struct array types, etc.
+% 
+% Outputs:
+%
+% vector  A vector containing the numerical values from the state (doubles)
+%
+% Example:
+% 
+% x = {[1 3; 4 2], struct('a', {5; 6}, 'bcd', {7:9; 10}), pi}
+% v = state_to_vector(x)
+% x2 = vector_to_state(v, x)
+% isequal(x, x2)
+%
+% See also: vector_to_state.
+%
+% Online doc: http://www.anuncommonlab.com/doc/odehybrid/state_to_vector.html
+%
+% Copyright 2014 An Uncommon Lab
+
+    % Ignore growth in loops. We're just going to have this problem.
+    %#ok<*AGROW>
+    
+	% If it's numbers, convert to doubles and store.
+    if isnumeric(state) || ischar(state)
+        
+        vector = double(state(:));
+        
+    % If it's a cell, convert each element.
+    elseif iscell(state)
+    
+        vector = [];
+        for k = 1:numel(state)
+            vector = [vector; state_to_vector(state{k})];
+        end
+        
+    % If it's a struct, convert each field.
+    elseif isstruct(state)
+        
+        vector = [];
+        fields = sort(fieldnames(state));
+        for n = 1:length(state)
+            for k = 1:length(fields)
+                vector = [vector; state_to_vector(state(n).(fields{k}))];
+            end
+        end
+
+    % Otherwise, we don't know what to do.
+    else
+        error('I don''t know to use the %s type.', class(state));
+    end
+
+end
diff --git a/Lib/various/odehybrid/vector_to_state.m b/Lib/various/odehybrid/vector_to_state.m
new file mode 100644
index 0000000..de32f5e
--- /dev/null
+++ b/Lib/various/odehybrid/vector_to_state.m
@@ -0,0 +1,77 @@
+function [state, count] = vector_to_state(vector, state, count)
+
+% vector_to_state
+%
+% Build a state from a state vector by recursively moving through the 
+% elements of a more complex example state (matrix, cell array, or struct),
+% placing the relevant numerical values from the vector into the 
+% appropriate places. This is used in odehybrid to convert a state vector
+% into the original complex setup of states. The reverse function is
+% state_to_vector.
+% 
+% state = vector_to_state(vector, state);
+% 
+% Inputs:
+% 
+% vector  A vector containing the numerical values from the object
+% state   A numeric, cell array, or struct array type representing the
+%         sizes and types for the values in vector
+% count   (Internal use only)
+% 
+% Outputs:
+%
+% state   A numeric, cell array, or struct array type
+% count   (Internal use only)
+%
+% Example:
+% 
+% x = {[1 3; 4 2], struct('a', {5; 6}, 'bcd', {7:9; 10}), pi}
+% v = state_to_vector(x)
+% x2 = vector_to_state(v, x)
+% isequal(x, x2)
+%
+% See also: state_to_vector.
+%
+% Online doc: http://www.anuncommonlab.com/doc/odehybrid/vector_to_state.html
+%
+% Copyright 2014 An Uncommon Lab
+
+    % If the user didn't input count, start from the beginnning.
+    if nargin < 3, count = 0; end;
+    
+    % If it's a cell, convert each element.
+    if iscell(state)
+        
+        for k = 1:numel(state)
+            [state{k}, count] = vector_to_state(vector, state{k}, count);
+        end
+        
+	% If it's a struct, convert and store in each field.
+    elseif isstruct(state)
+        
+        fields = sort(fieldnames(state));
+        for n = 1:length(state)
+            for k = 1:length(fields)
+                [state(n).(fields{k}), count] = vector_to_state( ...
+                                                  vector, ...
+                                                  state(n).(fields{k}), ...
+                                                  count);
+            end
+        end
+        
+	% If it's a bunch of numbers or chars, convert them from doubles to the
+	% appropriate type and store.
+    elseif isnumeric(state) || ischar(state)
+
+        % Since we overwrite state, it implicitly casts as necessary so
+        % that the output state will be the right type, whereas vector will
+        % always be a double.
+        state(:) = vector(count+(1:numel(state)));
+        count = count + numel(state);
+        
+	% Otherwise, we don't know what to do with it.
+    else
+        error('I don''t know to use the %s type.', class(state));
+    end
+
+end
diff --git a/Lib/various/odehybrid/vectors_to_states.m b/Lib/various/odehybrid/vectors_to_states.m
new file mode 100644
index 0000000..6a81c0c
--- /dev/null
+++ b/Lib/various/odehybrid/vectors_to_states.m
@@ -0,0 +1,150 @@
+function varargout = vectors_to_states(yv, varargin)
+% vectors_to_states
+%
+% Convert a state vector history to a series of states given those states
+% as inputs. If the total number of numeric states is ns and the number of
+% time steps represented is nt, then yv is nt-by-ns. The remaining
+% arguments are example states. The outputs will have the same form as the
+% example states, but will be arrays of these states, each nt long, with
+% appropriate values from yv.
+% 
+% When a state is a column or row vector of numeric types, the output will
+% be an nt-by-x array where x is length(state)
+%
+% When a state is an n-dimensional matrix (neither column nor vector), the
+% output will have the dimensions of the matrix concatenated on the first
+% unused dimension. That is, if the state is a-by-b, the output will be
+% a-by-b-by-nt.
+% 
+% When a state is a struct, the output will be a struct array of length nt.
+% 
+% Otherwise, the state will be a cell array of length nt, each containing
+% the example type filled in with values from yv.
+%
+% [y1, y2, ... yn] = vectors_to_states(yv, y1, y2, ... yn);
+% 
+% Inputs:
+%
+% yv              State vector history (nt-by-ns)
+% y1, y2, ... yn  Example states
+%
+% Outputs:
+% 
+% y1, y2, ... yn  State histories as described above
+%
+% Examples:
+% 
+% A history of 3 samples of a row vector:
+% 
+% yv = [state_to_vector([1 2]).'; ... % Sample 1
+%       state_to_vector([7 8]).'; ... % Sample 2
+%       state_to_vector([13 14]).'];  % Sample 3
+% y  = zeros(1, 2);                   % Example state
+% ys = vectors_to_states(yv, y)       % 3-by-2 output
+%
+% A history of 3 samples of 3 different states: a column vector, a cell
+% array, and a struct:
+% 
+% % Make the example states.
+% y1  = zeros(1, 3).';
+% y2  = {1, 2; 3 4};
+% y3  = struct('a', 0, 'b', 0);
+% 
+% % Make the history.
+% y1v = [state_to_vector([1 2 3].').'; ...
+%        state_to_vector([7 8 9].').'; ...
+%        state_to_vector([13 14 15].').'];
+% y2v = [state_to_vector({1 2; 7 8}).'; ...
+%        state_to_vector({3 4; 9 0}).'; ...
+%        state_to_vector({5 6; 1 2}).'];
+% y3v = [1 2; 3 4; 5 6];
+% yv  = [y1v, y2v, y3v];
+% 
+% % Get the state histories.
+% [y1s, y2s, y3s] = vectors_to_states(yv, y1, y2, y3)
+% 
+% See also: vector_to_state, state_to_vector.
+%
+% Online doc: http://www.anuncommonlab.com/doc/odehybrid/vectors_to_states.html
+%
+% Copyright 2014 An Uncommon Lab
+    
+    % Number of samples
+    ns = size(yv, 1);
+    
+    % Number of states
+    nx = length(varargin);
+    
+    % For each type of state...
+    count = 0;
+    varargout = cell(1, nx);
+    
+    % If there are no states, return empties.
+    if isempty(yv)
+        return;
+    end
+    
+    pre2008a = verLessThan('matlab', '8.1');
+    
+    for x = 1:length(varargin)
+        
+        % See if it's just a bunch of numbers (easy!).
+        if isnumeric(varargin{x}) || ischar(varargin{x})
+            
+            nv  = numel(varargin{x});
+            yvx = yv(:, count + (1:nv));
+            szs = size(varargin{x});
+            if length(szs) == 2 && any(szs(1:2) == 1)
+                if pre2008a
+                    varargout{x} = cast(yvx, class(varargin{x}));
+                else
+                    varargout{x} = cast(yvx, 'like', varargin{x});
+                end
+            else
+                if pre2008a
+                    varargout{x} = cast(reshape(yvx.', [szs ns]), ...
+                                        class(varargin{x}));
+                else
+                    varargout{x} = cast(reshape(yvx.', [szs ns]), ...
+                                        'like', varargin{x});
+                end
+            end
+            count = count + nv;
+            
+        % Cell arrays are pretty easy too.
+        elseif iscell(varargin{x})
+            
+            varargout{x} = cell(ns, 1);
+            dummy = state_to_vector(varargin{x});
+            yvx = yv(:, count + (1:length(dummy)));
+            for k = 1:ns
+                varargout{x}{k} = vector_to_state(yvx(k, :), varargin{x});
+            end
+            count = count + length(dummy);
+            
+        % Structures are wasteful but useful.
+        elseif isstruct(varargin{x}) && length(varargin{x}) == 1
+            
+            % Make the output struct like the input struct.
+            varargout{x} = varargin{x};
+            dummy = state_to_vector(varargin{x});
+            yvx = yv(:, count + (1:length(dummy)));
+            for k = 1:ns
+                varargout{x}(k,1) = vector_to_state(yvx(k,:), varargin{x});
+            end
+            count = count + length(dummy);
+            
+        else
+            
+            varargout{x} = cell(ns, 1);
+            dummy = state_to_vector(varargin{x});
+            yvx = yv(:, count + (1:length(dummy)));
+            for k = 1:ns
+                varargout{x}{k} = vector_to_state(yvx(k, :), varargin{x});
+            end
+            
+        end
+        
+    end
+
+end
diff --git a/Lib/various/odehybrid/version.txt b/Lib/various/odehybrid/version.txt
new file mode 100644
index 0000000..3eefcb9
--- /dev/null
+++ b/Lib/various/odehybrid/version.txt
@@ -0,0 +1 @@
+1.0.0
diff --git a/Lib/various/other/InitMultiMHE.m b/Lib/various/other/InitMultiMHE.m
new file mode 100644
index 0000000..6d652e9
--- /dev/null
+++ b/Lib/various/other/InitMultiMHE.m
@@ -0,0 +1,74 @@
+%% function 
+function [params_s, obs_s, time, SimParams, params, SIMstate, SIMinput, SIMmeasure] = InitMultiMHE(tstart,tend,Ts)
+
+    Ts_fast = Ts(1);
+    Ts_slow = Ts(2);
+    
+    % MHE fast
+    % set opt vars
+    system("sed -i 's/params.opt_vars =.*/params.opt_vars = [1:2];/' Lib/models/battery/params_battery_tushar.m");
+    pause(1);
+    system("sed -i 's/fil1 = .*/fil1 = 0;/' Lib/measure/filter_define.m");
+    pause(1);
+    system("sed -i 's/fil2 = .*/fil2 = 0;/' Lib/measure/filter_define.m");
+    pause(1);
+    [obs_fast, params_fast, ~] = setup_model_fast(tstart,tend,Ts_fast);
+
+    % MHE slow
+    % set opt vars
+    % 12:14 16:18 20:22
+    system("sed -i 's/params.opt_vars =.*/params.opt_vars = [1:2 8:10 12:14 16:18 20:22];/' Lib/models/battery/params_battery_tushar.m");
+    pause(1);
+    system("sed -i 's/fil1 = .*/fil1 = 0;/' Lib/measure/filter_define.m");
+    pause(1);
+    system("sed -i 's/fil2 = .*/fil2 = 0;/' Lib/measure/filter_define.m");
+    pause(1);
+    system("sed -i 's/fil3 = .*/fil3 = 0;/' Lib/measure/filter_define.m");
+    pause(1);
+    [obs_slow, params_slow, ~] = setup_model_slow(tstart,tend,Ts_slow);
+    
+    %%%% first guess %%%%
+    Ts_slower = min(Ts);
+    first_guess_flag = 1;
+    [obs, ~, ~] = setup_model_fast(tstart,tend,Ts_slower);
+    if first_guess_flag
+        first_guess;   
+        % noise on parameters - is it really necessary?
+        params_fast.X0 = obs.init.X_est.val(:,1);
+        params_slow.X0 = obs.init.X_est.val(:,1);
+        pert = (1+0.0*ones(obs.setup.dim_state,1));
+        obs.init.X_est.val(:,1) = obs.init.X_est.val(:,1).*pert;
+
+        % update objects        
+        obs_fast.init.X_est.val(:,1) = obs.init.X_est.val(:,1);
+        obs_slow.init.X_est.val(:,1) = obs.init.X_est.val(:,1);
+        obs_fast.init.params = obs_fast.setup.params.params_update(obs_fast.init.params,obs_fast.init.X_est(1).val(:,1));
+        obs_slow.init.params = obs_slow.setup.params.params_update(obs_slow.init.params,obs_slow.init.X_est(1).val(:,1));
+        obs_fast.setup.params = obs_fast.setup.params.params_update(obs_fast.setup.params,obs_fast.init.X_est(1).val(:,1));
+        obs_slow.setup.params = obs_slow.setup.params.params_update(obs_slow.setup.params,obs_slow.init.X_est(1).val(:,1));
+        obs_fast.init.cloud_X = obs.init.cloud_X;
+        obs_slow.init.cloud_X = obs.init.cloud_X;
+        obs_fast.init.cloud_Y = obs.init.cloud_Y;
+        obs_slow.init.cloud_Y = obs.init.cloud_Y;                
+    end
+    clear obs;        
+    
+    % set stuff
+    time = params_fast.time;
+    params_s = {params_fast, params_slow};
+    obs_s = {obs_fast, obs_slow};
+    
+    % set simulink
+    [obs_tmp, ~, SimParams] = setup_model;
+    params = params_s{1};
+    params.Ts_slower = Ts_slower;
+    noise = 1;
+    params.percNoise = noise*5e-2;
+    params.NoisePwr = noise*5e-2;
+    
+    obs_tmp.init.X_est.val = zeros(size(obs_tmp.init.X_est.val(:,1),1),length(obs_tmp.setup.time));
+    obs_tmp.init.input_story.val = zeros(obs_tmp.setup.params.dim_input,length(obs_tmp.setup.time)-1);
+    obs_tmp.init.Yhat_full_story.val = zeros(size(obs_tmp.init.X_est.val(:,1),1),1,length(obs_tmp.setup.time));
+    [SIMstate, SIMinput, SIMmeasure] = SaveToSimulink(obs_tmp,0);
+
+end
\ No newline at end of file
diff --git a/Lib/various/other/Lie_bracket.m b/Lib/various/other/Lie_bracket.m
new file mode 100644
index 0000000..9743479
--- /dev/null
+++ b/Lib/various/other/Lie_bracket.m
@@ -0,0 +1,10 @@
+%% function
+function Lf_g = Lie_bracket(f,g,x)
+
+    % reshape g
+    g = reshape(g,length(g),1);
+    
+    % lie brackert
+    Lf_g = gradient_sym(f,x)*g;
+
+end
\ No newline at end of file
diff --git a/Lib/various/other/SaveToSimulink.m b/Lib/various/other/SaveToSimulink.m
new file mode 100644
index 0000000..770cd04
--- /dev/null
+++ b/Lib/various/other/SaveToSimulink.m
@@ -0,0 +1,15 @@
+%%
+function [SIMstate, SIMinput, SIMmeasure] = SaveToSimulink(obs,time,runtime)
+
+    % create the bus to be provided to simulink    
+    if ~runtime
+        SIMstate = timeseries(obs.init.X_est.val,time);
+        SIMmeasure = timeseries(reshape(obs.init.Yhat_full_story.val(1,1,:),size(time)),time);
+    else
+        SIMstate = timeseries(obs.init.X_est_runtime.val,time);
+        SIMmeasure = timeseries(reshape(obs.init.Yhat_full_story_runtime.val(1,1,:),size(time)),time);
+    end
+    SIMinput = timeseries([0 obs.init.input_story.val(1,:)],time);
+    
+
+end
\ No newline at end of file
diff --git a/Lib/various/other/dcgain_ss.m b/Lib/various/other/dcgain_ss.m
new file mode 100644
index 0000000..1ab2c9c
--- /dev/null
+++ b/Lib/various/other/dcgain_ss.m
@@ -0,0 +1,6 @@
+%%
+function dc = dcgain_ss(A,B,C,D)
+
+    dc = -(C(1)*(A(4)*B(1)-A(3)*B(2)) + C(2)*(A(1)*B(2)-A(2)*B(1)))/(A(1)*A(4)-A(2)*A(3));
+    
+end
\ No newline at end of file
diff --git a/Lib/various/other/gradient_sym.m b/Lib/various/other/gradient_sym.m
new file mode 100644
index 0000000..9dbfbe8
--- /dev/null
+++ b/Lib/various/other/gradient_sym.m
@@ -0,0 +1,12 @@
+%% function
+function grad = gradient_sym(f,x)
+
+    % state dim
+    dim = length(x);
+    
+    % gradient
+    for i=1:dim
+       grad(1,i) = diff(f,x(i));
+    end
+
+end
\ No newline at end of file
diff --git a/Lib/various/other/model_analysis.m b/Lib/various/other/model_analysis.m
new file mode 100644
index 0000000..b842de2
--- /dev/null
+++ b/Lib/various/other/model_analysis.m
@@ -0,0 +1,158 @@
+%%
+function out = model_analysis(obs,option)
+    
+    if strcmp(option,'fromobs')
+        % true model
+        A = [obs.setup.params.A1 obs.setup.params.A2; obs.setup.params.A3 obs.setup.params.A4];
+        B = [obs.setup.params.B1; obs.setup.params.B2];
+        C=[obs.setup.params.C1 obs.setup.params.C2];
+        D = 0;
+        out.test = ss(A,B,C,D);
+
+        % estimated model
+        A_est = [obs.init.params.A1, obs.init.params.A2; obs.init.params.A3 obs.init.params.A4];
+        B_est = [obs.init.params.B1; obs.init.params.B2];
+        C_est = [obs.init.params.C1, obs.init.params.C2];
+        D_est = D;
+        out.test_est = ss(A_est,B_est,C_est,D_est);
+
+        % stabilised model
+        out.K = [obs.init.params.K1 obs.init.params.K2];
+        out.test_stable = ss(A_est+B_est*out.K,B_est,C_est,D_est);
+
+        % plots
+        figure()
+        rlocus(out.test)
+        hold on
+        rlocus(out.test_est)
+        rlocus(out.test_stable)
+    elseif strcmp(option,'routh')               
+        
+        % tf symbol
+        syms s
+        assume(s,'real')
+        
+        %%% Plant
+        out.omega = 2*pi*10;
+        out.rho = 0.5;
+        out.test = tf([out.omega^2],[1 2*out.omega*out.rho out.omega^2]);
+                
+        [A,B,C,D] = tf2ss(out.test.num{1},out.test.den{1});
+        out.test_ss = ss(A,B,C,D);
+        
+        out.test_ss_obsv = obsv(out.test_ss);
+        out.test_ss_ctrb = ctrb(out.test_ss);
+        
+        Ap = out.test_ss.A;
+        Bp = out.test_ss.B;
+        Cp = out.test_ss.C;
+        Dp = out.test_ss.D;
+        out.P = ss(Ap,Bp,Cp,Dp);
+        
+        % poly
+        out.Gp = simplify(vpa(Cp*pinv((s*eye(size(Ap))-Ap))*Bp+Dp));
+        [out.Nump, out.Denp] = numden(out.Gp);
+        
+        % controller
+        syms a0 a1 b0 b1 d0
+        assume(a0,'real')
+        assume(a1,'real')
+        assume(b0,'real')
+        assume(b1,'real')
+        assume(d0,'real')
+        out.a0 = a0;
+        out.a1 = a1;
+        out.b0 = b0;
+        out.b1 = b1;
+        out.d0 = d0;
+        
+        Ac = [0 1; a0 a1];
+        Bc = [b0; b1];
+        Cc = [1 0];
+        Dc = d0;
+        out.Ac = Ac;
+        out.Bc = Bc;
+        out.Cc = Cc;
+        out.Dc = Dc;
+        out.Gc = simplify(vpa(Cc*pinv((s*eye(size(Ac))-Ac))*Bc+Dc));
+        [out.Numc, out.Denc] = numden(out.Gc);
+        
+        % total model
+        out.A = [Ap-Bp*Cp*Dc, Bp*Cc; -Bc*Cp, Ac];
+        out.B = [Bp*Dc; Bc];
+        out.C = [Cp, [0 0]; -Dc*Cp, Cc];
+        out.D = [0; Dc];
+        out.poli = simplify(out.Nump*out.Numc + out.Denp*out.Denc);
+        out.poli_coeffs = coeffs(out.poli,s);
+        
+        % create example
+        Ac_es = double(subs(out.Ac,[a0 a1],[10 20]));
+        Bc_es = double(subs(out.Bc,[b0 b1],[30 60]));
+        Cc_es = Cc;
+        Dc_es = double(subs(out.Dc,d0,50));
+        Gc_es_ss = ss(Ac_es,Bc_es,Cc_es,Dc_es);
+        Gc_es_tf = subs(out.Gc,[a0 a1 b0 b1 d0],[10 20 30 60 50]);
+        [Gc_es_num,Gc_es_den] = numden(Gc_es_tf);
+        Gc_es_num = double(coeffs(Gc_es_num,s));
+        Gc_es_den = double(coeffs(Gc_es_den,s));
+        Gc_es_tf = tf(Gc_es_num,Gc_es_den);
+                        
+        % stability
+        out.RE = simplify(myRouth(out.poli_coeffs));
+        
+    elseif strcmp(option,'rlocus')
+        
+        %%% Plant
+        Ap = [obs.init.params.A1 obs.init.params.A2; obs.init.params.A3 obs.init.params.A4];
+        Bp = [obs.init.params.B1; obs.init.params.B2]; 
+        Cp = [obs.init.params.C1 obs.init.params.C2];
+        Dp = 0;       
+        out.test_ss = ss(Ap,Bp,Cp,Dp);
+        
+        out.test_ss_obsv = obsv(out.test_ss);
+        out.test_ss_ctrb = ctrb(out.test_ss);        
+        
+        % estimated plant
+        Ap = [0 1; obs.init.params.a0est obs.init.params.a1est];
+        Bp = [obs.init.params.b0est; obs.init.params.b1est]; 
+        Cp = [obs.init.params.c0est obs.init.params.c1est];
+        Dp = obs.init.params.d0est;
+        out.test_ss_est = ss(Ap,Bp,Cp,Dp);
+        
+        %%% controller
+        Ac = [0 1; obs.init.params.a0 obs.init.params.a1];
+        Bc = [obs.init.params.b0; obs.init.params.b1]; 
+        Cc = [obs.init.params.c0 obs.init.params.c1];
+        Dc = obs.init.params.d0;
+        out.ctrl_ss = ss(Ac,Bc,Cc,Dc);
+                
+        %%% FB system
+        Af = [(Ap - Bp*Cp*Dc), (Bp*Cc); (-Bc*Cp), (Ac)];
+        Bf = [(Bp*Dc); Bc];
+        Cf = [(Cp), zeros(1,2); (-Dc*Cp), (Cc)];
+        Df = [0; (Dc)];
+        out.fb_ss = ss(Af,Bf,Cf,Df);        
+        
+        %%% plots
+        figure(1)
+        rlocus(out.test_ss);
+        hold on
+        rlocus(out.test_ss_est)
+        figure(2)
+        bode(out.test_ss);
+        hold on
+        bode(out.test_ss_est)
+        figure(3)
+        rlocus(out.ctrl_ss)       
+        figure(4)
+        bode(out.test_ss,out.ctrl_ss)
+        figure(5)
+        bode(out.fb_ss);
+
+        
+    else
+        
+        error('Wrong option');
+    end
+
+end
\ No newline at end of file
diff --git a/Lib/various/other/nonlcon_fcn.m b/Lib/various/other/nonlcon_fcn.m
new file mode 100644
index 0000000..9a4bbdf
--- /dev/null
+++ b/Lib/various/other/nonlcon_fcn.m
@@ -0,0 +1,33 @@
+%% fcn
+function [c, ceq] = nonlcon_fcn(xopt,xnonopt,obs)    
+    
+    % create full state vector
+    x = zeros(obs.setup.dim_state,1);
+    x(obs.setup.opt_vars) = xopt;
+    x(obs.setup.nonopt_vars) = xnonopt(1).val;  
+    
+    % tolerance
+    tol = 1e0*obs.init.myoptioptions.ConstraintTolerance;
+    
+    % negative poles    
+    c = [];
+%     for traj = 1:obs.init.params.Ntraj
+%         sys_sz = size(obs.init.params.sys_pert(traj).sys_CL_All.A,1);
+%         tmp = -1e2*ones(sys_sz,1);
+%         minss = minreal(obs.init.params.sys_pert(traj).sys_CL_All,[],false);
+%         eig_sys = (real(eig(minss)) + tol);
+%         tmp(1:length(eig_sys)) = eig_sys;
+%         c = [c; tmp];        
+%     end
+    
+    % gamma negative
+%     c = [c; -(x(obs.init.params.GammaPos) - tol)];
+    
+    % PSI roots < 0
+%     tmp = (real(roots(x(obs.init.params.PsiPos))) + tol);
+%     c = [c; tmp];
+            
+    % cons
+    ceq = [];
+
+end
\ No newline at end of file
diff --git a/Lib/various/plots/extra_plots.m b/Lib/various/plots/extra_plots.m
new file mode 100644
index 0000000..556c403
--- /dev/null
+++ b/Lib/various/plots/extra_plots.m
@@ -0,0 +1,41 @@
+
+% Define file paths for the four datasets
+filepaths = {
+    '/data/CALCE/INR_18650/BJDST/SP2_25C_BJDST/11_12_2015_SP20-2_BJDST_80SOC.xlsx';
+    '/data/CALCE/INR_18650/US06/SP2_25C_US06/11_11_2015_SP20-2_US06_80SOC.xlsx';
+    '/data/CALCE/INR_18650/FUDS/SP2_25C_FUDS/11_06_2015_SP20-2_FUDS_80SOC.xlsx';
+    '/data/CALCE/INR_18650/DST/SP2_25C_DST/11_05_2015_SP20-2_DST_80SOC.xlsx'
+};
+
+% Define sheet names for each dataset (if different, update accordingly)
+sheet_name = 'Channel_1-008';
+
+% Define labels for the subplots
+labels = {'BJDST', 'US06', 'FUDS', 'DST'};
+
+figure;
+
+for i = 1:length(filepaths)
+    % Read data from each file
+    input_output_signal_data = readtable(filepaths{i}, 'Sheet', sheet_name);
+
+    % Find the starting index of step 7
+    startIndex = find(input_output_signal_data.Step_Index == 7, 1, 'first');
+    extracted_data = input_output_signal_data(startIndex:end, :);
+
+    % Extract Date_Time and Current_A_
+    Date_Time = extracted_data.Date_Time;
+    Current_A_ = extracted_data.Current_A_;
+    
+    % Convert Date_Time to seconds from the start
+    Date_Time = seconds(Date_Time - Date_Time(1));
+    
+    % Plotting
+    subplot(2, 2, i);
+    plot(Date_Time, Current_A_, 'LineWidth', 1); % Plot with thicker line
+    xlabel('time [s]');
+    ylabel('current [A]');
+    % title(labels{i});
+    grid on;
+    xlim([0, max(Date_Time)]); % Ensuring x-axis starts from 0
+end
diff --git a/Lib/various/plots/plot_3Dobject.m b/Lib/various/plots/plot_3Dobject.m
new file mode 100644
index 0000000..a1086eb
--- /dev/null
+++ b/Lib/various/plots/plot_3Dobject.m
@@ -0,0 +1,559 @@
+%% SIMULATION_GENERAL_V3
+% file: simulation_general_v3.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: function to setup and use the MHE out.observer on general model
+% INPUT: none
+% OUTPUT: params,out.obs
+% plot results for control design
+function plot_3Dobject(obj,varargin)
+    
+    set(0,'DefaultFigureWindowStyle','docked');            
+    
+    fontsize = 20;
+    fig_count = 0;
+    
+    if length(varargin) == 1
+        params = varargin{1};
+    else
+        params = obj.init.params;
+    end
+    
+    %%%% plot position estimation %%%
+    try
+        fig_count = fig_count+1;
+        figure(fig_count)            
+        sgtitle('Position estimation')
+        ax = zeros(1,3);
+        for i=1:length(params.pos_p)
+            subplot(length(params.pos_p),1,i);
+            hold on
+            grid on
+            box on
+    
+            % indicize axes        
+            ax(i)=subplot(length(params.pos_p),1,i);     
+            
+            for traj=1:obj.setup.Ntraj            
+                plot(obj.setup.time,obj.init.X(traj).val(params.pos_p(i),:),'LineWidth',2);
+                set(gca,'ColorOrderIndex',traj)
+                plot(obj.setup.time,obj.init.X_est(traj).val(params.pos_p(i),:),'--','LineWidth',1);                                                                                                  
+            end
+
+            % test
+            if i==3
+                plot(obj.setup.time,obj.init.reference_story(1).val(1,:),'k','LineWidth',1.5);
+            end
+            
+            % labels
+            set(gca,'fontsize', fontsize)         
+            ylabel(['p_',num2str(obj.setup.plot_vars(i))])
+        end
+        %linkaxes(ax);
+        legend('True','Est')   
+        xlabel(['time [s]']) 
+    catch
+        close
+        fig_count = fig_count -1;
+    end
+
+    %%% plot position estimation
+    try
+        fig_count = fig_count+1;
+        figure(fig_count)            
+        sgtitle('Velocity estimation')
+        ax = zeros(1,3);
+        for i=1:length(params.pos_v)
+            subplot(length(params.pos_v),1,i);
+            hold on
+            grid on
+            box on
+    
+            % indicize axes        
+            ax(i)=subplot(length(params.pos_v),1,i);     
+            
+            for traj=1:obj.setup.Ntraj            
+                plot(obj.setup.time,obj.init.X(traj).val(params.pos_v(i),:),'LineWidth',2);
+                set(gca,'ColorOrderIndex',traj)
+                plot(obj.setup.time,obj.init.X_est(traj).val(params.pos_v(i),:),'--','LineWidth',1);                                                                                    
+            end
+            
+            % labels
+            set(gca,'fontsize', fontsize)         
+            ylabel(['v_',num2str(obj.setup.plot_vars(i))])
+        end
+        legend('True','Est') 
+        xlabel(['time [s]'])
+        %linkaxes(ax);
+    catch
+        close
+        fig_count = fig_count -1;
+    end
+
+    %%% plot acceleration estimation
+    try
+        fig_count = fig_count+1;
+        figure(fig_count)            
+        sgtitle('Acceleration estimation')
+        ax = zeros(1,3);
+        for i=1:length(params.pos_acc)
+            subplot(length(params.pos_acc),1,i);
+            hold on
+            grid on
+            box on
+    
+            % indicize axes        
+            ax(i)=subplot(length(params.pos_acc),1,i);     
+            
+            for traj=1:obj.setup.Ntraj            
+                plot(obj.setup.time,obj.init.X(traj).val(params.pos_acc(i),:),'LineWidth',2);
+                set(gca,'ColorOrderIndex',traj)
+                plot(obj.setup.time,obj.init.X_est(traj).val(params.pos_acc(i),:),'--','LineWidth',1);                                                                                  
+            end
+            
+            % labels
+            set(gca,'fontsize', fontsize)         
+            ylabel(['a_',num2str(obj.setup.plot_vars(i))])
+        end
+        legend('True','Est')  
+        xlabel(['time [s]'])
+        %linkaxes(ax);
+    catch
+        close
+        fig_count = fig_count -1;
+    end
+
+    %%% plot bias estimation
+    try
+        fig_count = fig_count+1;
+        figure(fig_count)            
+        sgtitle('Bias estimation')
+        ax = zeros(1,3);
+        for i=1:length(params.pos_bias)
+            subplot(length(params.pos_bias),1,i);
+            hold on
+            grid on
+            box on
+    
+            % indicize axes        
+            ax(i)=subplot(length(params.pos_bias),1,i);     
+            
+            for traj=1:obj.setup.Ntraj            
+                plot(obj.setup.time,obj.init.X(traj).val(params.pos_bias(i),:),'LineWidth',2);
+                set(gca,'ColorOrderIndex',traj)
+                plot(obj.setup.time,obj.init.X_est(traj).val(params.pos_bias(i),:),'--','LineWidth',1);                                                                                   
+            end
+            
+            % labels
+            set(gca,'fontsize', fontsize)         
+            ylabel(['b_',num2str(obj.setup.plot_vars(i))])
+        end
+        legend('True','Est') 
+        xlabel(['time [s]'])
+        %linkaxes(ax);
+    catch
+        close
+        fig_count = fig_count -1;
+    end
+
+    %%% plot Quaternion estimation
+    try
+        fig_count = fig_count+1;
+        figure(fig_count)            
+        sgtitle('Quaternion estimation')
+        ax = zeros(1,3);
+        for i=1:length(params.pos_quat)
+            subplot(length(params.pos_quat),1,i);
+            hold on
+            grid on
+            box on
+    
+            % indicize axes        
+            ax(i)=subplot(length(params.pos_quat),1,i);     
+            
+            for traj=1:obj.setup.Ntraj            
+                plot(obj.setup.time,obj.init.X(traj).val(params.pos_quat(i),:),'LineWidth',2);
+                set(gca,'ColorOrderIndex',traj)
+                plot(obj.setup.time,obj.init.X_est(traj).val(params.pos_quat(i),:),'--','LineWidth',1);                                                                         
+            end
+            
+            % labels
+            set(gca,'fontsize', fontsize)         
+            ylabel(['q_',num2str(obj.setup.plot_vars(i))])
+        end
+        legend('True','Est')            
+        xlabel(['time [s]'])
+        %linkaxes(ax);
+    catch
+        close
+        fig_count = fig_count -1;
+    end
+
+    %%% plot omega estimation
+    try
+        fig_count = fig_count+1;
+        figure(fig_count)            
+        sgtitle('Omega estimation')
+        ax = zeros(1,3);
+        for i=1:length(params.pos_omega)
+            subplot(length(params.pos_omega),1,i);
+            hold on
+            grid on
+            box on
+    
+            % indicize axes        
+            ax(i)=subplot(length(params.pos_omega),1,i);     
+            
+            for traj=1:obj.setup.Ntraj            
+                plot(obj.setup.time,obj.init.X(traj).val(params.pos_omega(i),:),'LineWidth',2);
+                set(gca,'ColorOrderIndex',traj)
+                plot(obj.setup.time,obj.init.X_est(traj).val(params.pos_omega(i),:),'--','LineWidth',1);                                                                                   
+            end
+            
+            % labels
+            set(gca,'fontsize', fontsize)         
+            ylabel(['\omega_',num2str(obj.setup.plot_vars(i))])
+        end
+        legend('True','Est') 
+        xlabel(['time [s]'])
+        %linkaxes(ax);
+    catch
+        close
+        fig_count = fig_count -1;
+    end
+
+    %%% plot jerk estimation
+    try
+        fig_count = fig_count+1;
+        figure(fig_count)            
+        sgtitle('Jerk estimation')
+        ax = zeros(1,3);
+        for i=1:length(params.pos_jerk)
+            subplot(length(params.pos_jerk),1,i);
+            hold on
+            grid on
+            box on
+    
+            % indicize axes        
+            ax(i)=subplot(length(params.pos_jerk),1,i);     
+            
+            for traj=1:obj.setup.Ntraj            
+                plot(obj.setup.time,obj.init.X(traj).val(params.pos_jerk(i),:),'LineWidth',2);
+                set(gca,'ColorOrderIndex',traj)
+                plot(obj.setup.time,obj.init.X_est(traj).val(params.pos_jerk(i),:),'--','LineWidth',1);                                                                                  
+            end
+            
+            % labels
+            set(gca,'fontsize', fontsize)         
+            ylabel(['j_',num2str(obj.setup.plot_vars(i))])
+        end
+        legend('True','Est')  
+        xlabel(['time [s]'])
+        %linkaxes(ax);
+    catch
+        close
+        fig_count = fig_count -1;
+    end
+
+    %%% plot alpha estimation
+    try
+        fig_count = fig_count+1;
+        figure(fig_count)            
+        sgtitle('Alpha estimation')
+        ax = zeros(1,3);
+        for i=1:length(params.pos_alpha)
+            subplot(length(params.pos_alpha),1,i);
+            hold on
+            grid on
+            box on
+    
+            % indicize axes        
+            ax(i)=subplot(length(params.pos_alpha),1,i);     
+            
+            for traj=1:obj.setup.Ntraj            
+                plot(obj.setup.time,obj.init.X(traj).val(params.pos_alpha(i),:),'LineWidth',2);
+                set(gca,'ColorOrderIndex',traj)
+                plot(obj.setup.time,obj.init.X_est(traj).val(params.pos_alpha(i),:),'--','LineWidth',1);                                                                                
+            end
+            
+            % labels
+            set(gca,'fontsize', fontsize)         
+            ylabel(['\alpha_',num2str(obj.setup.plot_vars(i))])
+        end
+        legend('True','Est')     
+        xlabel(['time [s]'])
+        %linkaxes(ax);
+    catch
+        close
+        fig_count = fig_count -1;
+    end
+
+    %%% plot bias vel estimation
+    try
+        fig_count = fig_count+1;
+        figure(fig_count)            
+        sgtitle('Bias vel estimation')
+        ax = zeros(1,3);
+        for i=1:length(params.pos_bias_vel)
+            subplot(length(params.pos_bias_vel),1,i);
+            hold on
+            grid on
+            box on
+    
+            % indicize axes        
+            ax(i)=subplot(length(params.pos_bias_vel),1,i);     
+            
+            for traj=1:obj.setup.Ntraj            
+                plot(obj.setup.time,obj.init.X(traj).val(params.pos_bias_vel(i),:),'LineWidth',2);
+                set(gca,'ColorOrderIndex',traj)
+                plot(obj.setup.time,obj.init.X_est(traj).val(params.pos_bias_vel(i),:),'--','LineWidth',1);                                                                         
+            end
+            
+            % labels
+            set(gca,'fontsize', fontsize)         
+            ylabel(['bv_',num2str(obj.setup.plot_vars(i))])
+        end
+        legend('True','Est')            
+        xlabel(['time [s]'])    
+        %linkaxes(ax);
+    catch
+        close
+        fig_count = fig_count -1;
+    end
+    
+    %%%% plot pos measure data %%%%   
+    try
+        fig_count = fig_count+1;
+        figure(fig_count)            
+        sgtitle('pos measure')
+        ax = zeros(1,3);
+        for i=1:length(params.pos_p_out)
+            subplot(length(params.pos_p_out),1,i);
+            hold on
+            grid on
+            box on
+    
+            % indicize axes        
+            ax(i)=subplot(length(params.pos_p_out),1,i);    
+
+            % down sampling instants
+            WindowTime = obj.setup.time(obj.init.temp_time);
+            
+            for traj=1:obj.setup.Ntraj            
+                plot(obj.setup.time,squeeze(obj.init.Ytrue_full_story(traj).val(1,params.pos_p_out(i),:)),'LineWidth',2);
+                set(gca,'ColorOrderIndex',traj)
+                plot(obj.setup.time,squeeze(obj.init.Y_full_story(traj).val(1,params.pos_p_out(i),:)),'--','LineWidth',1);          
+
+                set(gca,'ColorOrderIndex',traj)
+                % plot target values    
+                try
+                    data = reshape(obj.init.Y_full_story(traj).val(1,obj.setup.params.pos_p_out(i),obj.init.temp_time),1,length(WindowTime));
+                    plot(WindowTime,data,'o','MarkerSize',5);
+                catch 
+                    disp('CHECK T_END OR AYELS CONDITION - LOOKS LIKE NO OPTIMISATION HAS BEEN RUN')
+                end
+            end
+            
+            % labels
+            set(gca,'fontsize', fontsize)         
+            ylabel(['p_',num2str(obj.setup.plot_vars(i))])
+        end
+        legend('True','Meas')            
+        xlabel(['time [s]'])   
+        %linkaxes(ax);
+    catch
+        close
+        fig_count = fig_count -1;
+    end
+
+    %%%% plot quat measure data %%%%  
+    try
+        fig_count = fig_count+1;
+        figure(fig_count)            
+        sgtitle('quat measure')
+        ax = zeros(1,3);
+        for i=1:length(params.pos_quat_out)
+            subplot(length(params.pos_quat_out),1,i);
+            hold on
+            grid on
+            box on
+    
+            % indicize axes        
+            ax(i)=subplot(length(params.pos_quat_out),1,i);    
+
+            % down sampling instants
+            WindowTime = obj.setup.time(obj.init.temp_time);
+            
+            for traj=1:obj.setup.Ntraj            
+                plot(obj.setup.time,squeeze(obj.init.Ytrue_full_story(traj).val(1,params.pos_quat_out(i),:)),'LineWidth',2);
+                set(gca,'ColorOrderIndex',traj)
+                plot(obj.setup.time,squeeze(obj.init.Y_full_story(traj).val(1,params.pos_quat_out(i),:)),'--','LineWidth',1);     
+
+                set(gca,'ColorOrderIndex',traj)
+                % plot target values    
+                try
+                    data = reshape(obj.init.Y_full_story(traj).val(1,obj.setup.params.pos_quat_out(i),obj.init.temp_time),1,length(WindowTime));
+                    plot(WindowTime,data,'o','MarkerSize',5);
+                catch 
+                    disp('CHECK T_END OR AYELS CONDITION - LOOKS LIKE NO OPTIMISATION HAS BEEN RUN')
+                end
+            end
+            
+            % labels
+            set(gca,'fontsize', fontsize)         
+            ylabel(['q_',num2str(obj.setup.plot_vars(i))])
+        end
+        legend('True','Meas')           
+        xlabel(['time [s]'])
+        %linkaxes(ax);
+    catch
+        close
+        fig_count = fig_count -1;
+    end
+
+    %%%% plot acc measure data %%%%  
+    try
+        fig_count = fig_count+1;
+        figure(fig_count)            
+        sgtitle('acc measure')
+        ax = zeros(1,3);
+        for i=1:length(params.pos_acc_out)
+            subplot(length(params.pos_acc_out),1,i);
+            hold on
+            grid on
+            box on
+    
+            % indicize axes        
+            ax(i)=subplot(length(params.pos_acc_out),1,i);   
+
+            % down sampling instants
+            WindowTime = obj.setup.time(obj.init.temp_time);
+            
+            for traj=1:obj.setup.Ntraj            
+                plot(obj.setup.time,squeeze(obj.init.Ytrue_full_story(traj).val(1,params.pos_acc_out(i),:)),'LineWidth',2);
+                set(gca,'ColorOrderIndex',traj)
+                plot(obj.setup.time,squeeze(obj.init.Y_full_story(traj).val(1,params.pos_acc_out(i),:)),'--','LineWidth',1);    
+
+                set(gca,'ColorOrderIndex',traj)
+                % plot target values    
+                try
+                    data = reshape(obj.init.Y_full_story(traj).val(1,obj.setup.params.pos_acc_out(i),obj.init.temp_time),1,length(WindowTime));
+                    plot(WindowTime,data,'o','MarkerSize',5);
+                catch 
+                    disp('CHECK T_END OR AYELS CONDITION - LOOKS LIKE NO OPTIMISATION HAS BEEN RUN')
+                end
+            end
+            
+            % labels
+            set(gca,'fontsize', fontsize)         
+            ylabel(['a_',num2str(obj.setup.plot_vars(i))])
+        end
+        legend('True','Meas')            
+        xlabel(['time [s]'])
+        %linkaxes(ax);
+    catch
+        close
+        fig_count = fig_count -1;
+    end
+
+    %%%% plot omega measure data %%%% 
+    try
+        fig_count = fig_count+1;
+        figure(fig_count)            
+        sgtitle('omega measure')
+        ax = zeros(1,3);
+        for i=1:length(params.pos_omega_out)
+            subplot(length(params.pos_omega_out),1,i);
+            hold on
+            grid on
+            box on
+    
+            % indicize axes        
+            ax(i)=subplot(length(params.pos_omega_out),1,i);     
+
+            % down sampling instants
+            WindowTime = obj.setup.time(obj.init.temp_time);
+            
+            for traj=1:obj.setup.Ntraj            
+                plot(obj.setup.time,squeeze(obj.init.Ytrue_full_story(traj).val(1,params.pos_omega_out(i),:)),'LineWidth',2);
+                set(gca,'ColorOrderIndex',traj)
+                plot(obj.setup.time,squeeze(obj.init.Y_full_story(traj).val(1,params.pos_omega_out(i),:)),'--','LineWidth',1);   
+
+                set(gca,'ColorOrderIndex',traj)
+                % plot target values    
+                try
+                    data = reshape(obj.init.Y_full_story(traj).val(1,obj.setup.params.pos_omega_out(i),obj.init.temp_time),1,length(WindowTime));
+                    plot(WindowTime,data,'o','MarkerSize',5);
+                catch 
+                    disp('CHECK T_END OR AYELS CONDITION - LOOKS LIKE NO OPTIMISATION HAS BEEN RUN')
+                end
+            end
+            
+            % labels
+            set(gca,'fontsize', fontsize)         
+            ylabel(['\omega_',num2str(obj.setup.plot_vars(i))])
+        end             
+        xlabel(['time [s]'])
+        %linkaxes(ax);  
+    catch
+        close
+        fig_count = fig_count -1;
+    end
+    
+    try
+        %%% check distances %%%
+        fig_count = fig_count+1;
+        figure(fig_count)    
+        for n=1:obj.init.params.Nanchor
+            ax(n) = subplot(obj.init.params.Nanchor,1,n);
+            hold on
+            grid on  
+            for traj=1:obj.setup.Ntraj
+                plot(obj.setup.time(obj.init.params.UWB_pos),squeeze(obj.init.Y_full_story(traj).val(1,obj.init.params.pos_dist_out(n),obj.init.params.UWB_pos)),'LineWidth',2);
+                set(gca,'ColorOrderIndex',traj)
+                plot(obj.setup.time(obj.init.params.UWB_pos),squeeze(obj.init.Yhat_full_story(traj).val(1,obj.init.params.pos_dist_out(n),obj.init.params.UWB_pos)),':','LineWidth',2);
+                set(gca,'ColorOrderIndex',traj)
+                plot(obj.setup.time(obj.init.params.UWB_pos),squeeze(obj.init.Ytrue_full_story(traj).val(1,obj.init.params.pos_dist_out(n),obj.init.params.UWB_pos)),'--','LineWidth',2);
+            end        
+            ylabel(['d_',num2str(n)])        
+            set(gca,'fontsize', fontsize)
+        end   
+        xlabel('time [s]')
+        legend('meas','opt','true');
+    catch
+        close
+        fig_count = fig_count -1;
+    end
+
+    try
+        %%% rover trajectory
+        fig_count = fig_count+1;
+        figure(fig_count)
+        hold on
+        grid on
+        % plot hills
+        pos_p = obj.init.params.pos_p;
+        surf(params.X_gauss,params.Y_gauss,params.G_gauss,'FaceAlpha',0.5,'EdgeColor','none');
+        plot3(obj.init.X(1).val(pos_p(1),:),obj.init.X(1).val(pos_p(2),:),obj.init.reference_story(1).val(1,:),'k','LineWidth',2);
+        % plot anchors
+        P_a(1,:) = obj.init.params.pos_anchor(1):3:obj.init.params.pos_anchor(end);
+        P_a(2,:) = obj.init.params.pos_anchor(2):3:obj.init.params.pos_anchor(end); 
+        P_a(3,:) = obj.init.params.pos_anchor(3):3:obj.init.params.pos_anchor(end); 
+        for i=1:obj.setup.params.Nanchor
+            plot3(obj.init.X_est(1).val(P_a(1,i),:),obj.init.X_est(1).val(P_a(2,i),:),obj.init.X_est(1).val(P_a(3,i),:),'ko','MarkerSize',10);
+        end
+        % plot rover        
+        for traj=1:obj.setup.Ntraj 
+            plot3(obj.init.X_est(traj).val(pos_p(1),:),obj.init.X_est(traj).val(pos_p(2),:),obj.init.X_est(traj).val(pos_p(3),:),'--','LineWidth',1.5);
+            set(gca,'ColorOrderIndex',traj)
+            plot3(obj.init.X(traj).val(pos_p(1),:),obj.init.X(traj).val(pos_p(2),:),obj.init.X(traj).val(pos_p(3),:),'LineWidth',1.5);    
+        end
+        xlabel('X')
+        ylabel('Y')
+        set(gca,'fontsize', fontsize)
+
+    catch
+        close
+        fig_count = fig_count -1;
+    end
+end
\ No newline at end of file
diff --git a/Lib/various/plots/plot_TCV.m b/Lib/various/plots/plot_TCV.m
new file mode 100644
index 0000000..b9356d4
--- /dev/null
+++ b/Lib/various/plots/plot_TCV.m
@@ -0,0 +1,396 @@
+%%
+% plot results for control design
+    function plot_TCV(obj)
+
+        set(0,'DefaultFigureWindowStyle','docked');
+
+        fig_count = 0;
+
+        %%%% plot state estimation %%%
+        fig_count = fig_count+1;
+        figure(fig_count)
+        sgtitle('Ref and Control')
+        range = 1:(obj.setup.params.dim_state_r+obj.setup.params.dim_state_c);
+        for i=range
+            subplot(length(range),1,i-range(1)+1);
+            hold on
+            grid on
+            box on
+
+            for traj=1:obj.setup.Ntraj
+                if strcat(obj.setup.DataType,'simulated')
+                    plot(obj.setup.time,obj.init.X(traj).val(obj.setup.plot_vars(i),:),'b--');
+                end
+                plot(obj.setup.time,obj.init.X_est(traj).val(obj.setup.plot_vars(i),:),'r--');                                      
+
+                if strcat(obj.setup.DataType,'simulated')
+                    legend('True','Est')
+                else
+                    legend('Stored','Est','Runtime')
+                end
+            end
+
+            % labels
+            xlabel(['time [s]'])
+            ylabel(['x_',num2str(obj.setup.plot_vars(i))])
+        end
+        
+        %%%% plot state estimation %%%
+        fig_count = fig_count+1;
+        figure(fig_count)
+        sgtitle('Optimizer')
+        range = (obj.setup.params.dim_state_r+obj.setup.params.dim_state_c+1):(obj.setup.params.dim_state_r+obj.setup.params.dim_state_c+obj.setup.params.dim_state_op);
+        for i=range
+            subplot(length(range),1,i-range(1)+1);
+            hold on
+            grid on
+            box on
+
+            for traj=1:obj.setup.Ntraj
+                if strcat(obj.setup.DataType,'simulated')
+                    plot(obj.setup.time,obj.init.X(traj).val(obj.setup.plot_vars(i),:),'b--');
+                end
+                plot(obj.setup.time,obj.init.X_est(traj).val(obj.setup.plot_vars(i),:),'r--');                                      
+
+                if strcat(obj.setup.DataType,'simulated')
+                    legend('True','Est')
+                else
+                    legend('Stored','Est','Runtime')
+                end
+            end
+
+            % labels
+            xlabel(['time [s]'])
+            ylabel(['x_',num2str(obj.setup.plot_vars(i))])
+        end
+
+        fig_count = fig_count+1;
+        figure(fig_count)
+        sgtitle('Annihilator')
+        range = (obj.setup.params.dim_state_r+obj.setup.params.dim_state_c+obj.setup.params.dim_state_op+1):(obj.setup.params.dim_state_r+obj.setup.params.dim_state_c+obj.setup.params.dim_state_op+obj.setup.params.dim_state_an);
+        for i=range
+            subplot(length(range),1,i-range(1)+1);
+            hold on
+            grid on
+            box on
+
+            for traj=1:obj.setup.Ntraj
+                if strcat(obj.setup.DataType,'simulated')
+                    plot(obj.setup.time,obj.init.X(traj).val(obj.setup.plot_vars(i),:),'b--');
+                end
+                plot(obj.setup.time,obj.init.X_est(traj).val(obj.setup.plot_vars(i),:),'r--');                                      
+
+                if strcat(obj.setup.DataType,'simulated')
+                    legend('True','Est')
+                else
+                    legend('Stored','Est','Runtime')
+                end
+            end
+
+            % labels
+            xlabel(['time [s]'])
+            ylabel(['x_',num2str(obj.setup.plot_vars(i))])
+        end
+        
+        %%%% plot state estimation %%%
+        fig_count = fig_count+1;
+        figure(fig_count)
+        sgtitle('State estimation')
+        range = (obj.setup.params.dim_state_r+obj.setup.params.dim_state_c+obj.setup.params.dim_state_op+obj.setup.params.dim_state_an+1):(obj.setup.params.dim_state_r+...
+                obj.setup.params.dim_state_c+obj.setup.params.dim_state_op+obj.setup.params.dim_state_an+obj.setup.params.n);
+        for i=range
+            subplot(length(range),1,i-range(1)+1);
+            hold on
+            grid on
+            box on
+
+            for traj=1:obj.setup.Ntraj
+                if strcat(obj.setup.DataType,'simulated')
+                    plot(obj.setup.time,obj.init.X(traj).val(obj.setup.plot_vars(i),:),'b--');
+                end
+                plot(obj.setup.time,obj.init.X_est(traj).val(obj.setup.plot_vars(i),:),'r--');                                      
+
+                if strcat(obj.setup.DataType,'simulated')
+                    legend('True','Est')
+                else
+                    legend('Stored','Est','Runtime')
+                end
+            end
+
+            % labels
+            xlabel(['time [s]'])
+            ylabel(['x_',num2str(obj.setup.plot_vars(i))])
+        end
+
+        %%%% plot parameters estimation %%%
+        if ~isempty(obj.setup.plot_params)
+            fig_count = fig_count+1;
+            figure(fig_count)
+            sgtitle('Parameters estimation')
+            for i=1:length(obj.setup.plot_params)
+                subplot(length(obj.setup.plot_params),1,i);
+                hold on
+                grid on
+                box on
+
+                for traj=1:obj.setup.Ntraj
+                    if strcat(obj.setup.DataType,'simulated')
+                        plot(obj.setup.time,obj.init.X(traj).val(obj.setup.plot_params(i),:),'b--');
+                    end
+                    plot(obj.setup.time,obj.init.X_est(traj).val(obj.setup.plot_params(i),:),'r--');                                      
+
+                    if strcat(obj.setup.DataType,'simulated')
+                        legend('True','Est')
+                    else
+                        legend('Stored','Est','Runtime')
+                    end
+                end
+
+                % labels
+                xlabel(['time [s]'])
+                ylabel(['x_',num2str(obj.setup.plot_params(i))])
+            end
+        end
+
+        %%%% plot state estimation error %%%
+        fig_count = fig_count+1;
+        figure(fig_count)
+        sgtitle('Estimation error - components')
+
+        range = (obj.setup.params.dim_state_r+obj.setup.params.dim_state_c+obj.setup.params.dim_state_op+obj.setup.params.dim_state_an+1):(obj.setup.params.dim_state_r+...
+                obj.setup.params.dim_state_c+obj.setup.params.dim_state_op+obj.setup.params.dim_state_an+obj.setup.params.n);
+        for i=range
+            subplot(length(range),1,i-range(1)+1);
+            hold on
+            grid on
+            box on
+
+            for traj=1:obj.setup.Ntraj
+                % plot
+                est_error = obj.init.X(traj).val(obj.setup.plot_vars(i),:) - obj.init.X_est(traj).val(obj.setup.plot_vars(i),:);
+                
+                log_flag = 1;
+                if ~log_flag
+                    plot(obj.setup.time,est_error,'k','LineWidth',2);
+                else
+                    set(gca, 'YScale', 'log')
+                    plot(obj.setup.time,abs(est_error),'k','LineWidth',2);
+                end
+            end            
+
+            xlabel('time [s]')
+            ylabel(['\delta x_',num2str(obj.setup.plot_vars(i))])
+        end
+
+        %%%% plot parameters estimation error %%%
+        if ~isempty(obj.setup.plot_params)
+            fig_count = fig_count+1;
+            figure(fig_count)
+            sgtitle('Estimation error - parameters')
+
+            for i=1:length(obj.setup.plot_params)
+                subplot(length(obj.setup.plot_params),1,i);
+                hold on
+                grid on
+                box on
+
+                for traj=1:obj.setup.Ntraj
+                    % plot
+                    est_error = obj.init.X(1).val(obj.setup.plot_params(i),:) - obj.init.X_est(1).val(obj.setup.plot_params(i),:);
+                    log_flag = 1;
+                    if ~log_flag
+                        plot(obj.setup.time,est_error,'k','LineWidth',2);
+                    else
+                        set(gca, 'YScale', 'log')
+                        plot(obj.setup.time,abs(est_error),'k','LineWidth',2);
+                    end
+                end                
+
+                xlabel('time [s]')
+                ylabel(['\delta x_',num2str(obj.setup.plot_params(i))])
+            end
+        end
+
+        %%%% plot state estimation error - norm%%%
+        fig_count = fig_count+1;
+        figure(fig_count)
+        sgtitle('Estimation error state - norm')
+        hold on
+        grid on
+        box on
+        
+        est_error_norm = [];
+        for traj=1:obj.setup.Ntraj
+            % plot
+            for iter=1:obj.setup.Niter
+                est_error_norm(iter) = norm(obj.init.X(traj).val(obj.setup.plot_vars,iter) - obj.init.X_est(traj).val(obj.setup.plot_vars,iter));            
+            end
+
+            log_flag = 1;
+            if ~log_flag
+                plot(obj.setup.time,est_error_norm,'k','LineWidth',2);
+            else
+                set(gca, 'YScale', 'log')
+                plot(obj.setup.time,abs(est_error_norm),'k--','LineWidth',2);
+            end
+        end
+        
+        xlabel('time [s]')
+        ylabel('\delta x_norm') 
+
+        %%%% plot params estimation error - norm%%%
+        fig_count = fig_count+1;
+        figure(fig_count)
+        sgtitle('Estimation error params - norm')
+        hold on
+        grid on
+        box on
+
+        est_error_norm = [];
+        for traj=1:obj.setup.Ntraj
+            
+            % plot
+            for iter=1:obj.setup.Niter
+                est_error_norm(iter) = norm(obj.init.X(traj).val(obj.setup.plot_params,iter) - obj.init.X_est(traj).val(obj.setup.plot_params,iter));                                
+            end   
+            
+            log_flag = 1;
+            if ~log_flag
+                plot(obj.setup.time,est_error_norm,'k','LineWidth',2);
+            else
+                set(gca, 'YScale', 'log')
+                plot(obj.setup.time,abs(est_error_norm),'k--','LineWidth',2);
+            end
+        end                
+
+        xlabel('time [s]')
+        ylabel('\delta x_norm') 
+        
+        %%%% plot input difference %%%%
+        if ~isempty(obj.init.input_story)
+            fig_count = fig_count+1;
+            figure(fig_count)
+            sgtitle('Allocated input - absolute')
+
+            for i=1:obj.setup.dim_input    
+                subplot(obj.setup.dim_input,1,i)
+                hold on
+                grid on
+                box on
+
+                for traj=1:obj.setup.Ntraj
+                    % plot
+                    u_def = obj.init.input_default_story(traj).val(i,:);                    
+                    u_all = obj.init.input_story(traj).val(i,:);
+                    plot(obj.setup.time(1:length(u_def)),u_def,'k--','LineWidth',1.5);
+                    plot(obj.setup.time(1:length(u_all)),u_all,'r','LineWidth',1.5);
+
+                end                                                
+            end
+            xlabel('time [s]')
+            ylabel('inputs')
+            legend('u_{def}','u_{all}')
+        end
+        
+        %%%% plot input difference %%%%
+        if ~isempty(obj.init.input_story)
+            fig_count = fig_count+1;
+            figure(fig_count)
+            sgtitle('Allocated input - difference')
+
+            for i=1:obj.setup.dim_input
+                subplot(obj.setup.dim_input,1,i);
+                hold on
+                grid on
+                box on
+
+                for traj=1:obj.setup.Ntraj
+                    % plot
+                    est_error_all = obj.init.input_default_story(traj).val(i,:) - obj.init.input_story(traj).val(i,:);                    
+                    log_flag = 0;
+                    if ~log_flag
+                        plot(obj.setup.time(1:length(est_error_all)),est_error_all,'k','LineWidth',2);                        
+                    else
+                        set(gca, 'YScale', 'log')
+                        plot(obj.setup.time(1:length(est_error_all)),abs(est_error_all),'k','LineWidth',2);                        
+                    end
+                end                
+                
+                xlabel('time [s]')
+                ylabel(['\delta u_',num2str(i)])
+            end
+        end
+        
+        %%%% plot output q difference %%%%
+        if ~isempty(obj.init.input_story)
+            fig_count = fig_count+1;
+            figure(fig_count)
+            sgtitle('Reference output - difference')
+
+            for i=1:obj.setup.params.q
+                subplot(obj.setup.params.q,1,i);
+                hold on
+                grid on
+                box on
+
+                for traj=1:obj.setup.Ntraj
+                    % plot
+                    pos = obj.setup.params.q_pos(i);
+                    est_error_all = reshape(obj.init.Y_full_story(traj).val(1,pos,:),1,obj.setup.Niter) - reshape(obj.init.Yhat_full_story(traj).val(1,pos,:),1,obj.setup.Niter);
+                    plot(obj.setup.time,est_error_all,'r','LineWidth',2);
+                end                
+                
+                xlabel('time [s]')
+                ylabel(['\delta yq_',num2str(i)])
+            end
+        end
+
+        %%%% plot windowed data %%%%
+        fig_count = fig_count+1;
+        figure(fig_count)        
+        sgtitle('Sampled measured')
+        ax = zeros(1,3);
+        for k=1:obj.setup.dim_out
+
+            % number fo subplots depending on the output dimension
+            n_subplot = obj.setup.dim_out;
+
+            % indicize axes
+            ax_index = k;
+            ax(ax_index)=subplot(n_subplot,1,ax_index);
+
+            % hold on on plots
+            hold on
+            grid on
+
+            % dpwn sampling instants
+            WindowTime = obj.setup.time(obj.init.temp_time);
+
+            for traj=1:obj.setup.Ntraj
+                % plot true values
+                y_meas = reshape(obj.init.Ytrue_full_story(traj).val(1,k,:),size(obj.setup.time));
+                plot(obj.setup.time,y_meas,'m--')
+                
+                % plot ref values
+                y_ref = reshape(obj.init.Y_full_story(traj).val(1,k,:),size(obj.setup.time));
+                plot(obj.setup.time,y_ref,'k')
+
+                % plot estimated values
+                yhat = reshape(obj.init.Yhat_full_story(traj).val(1,k,:),size(obj.setup.time));
+                plot(obj.setup.time,yhat,'r--','LineWidth',1.5)
+
+
+                try
+                    data = reshape(obj.init.target_story(traj).val(1,k,obj.init.temp_time),1,length(WindowTime));
+                catch 
+                    disp('CHECK T_END OR AYELS CONDITION - LOOKS LIKE NO OPTIMISATION HAS BEEN RUN')
+                end
+
+                ylabel(strcat('y_',num2str(k)));
+                xlabel('simulation time [s]');
+                legend('real','est')
+            end
+        end
+        linkaxes(ax,'x');       
+
+    end
\ No newline at end of file
diff --git a/Lib/various/plots/plot_general.m b/Lib/various/plots/plot_general.m
new file mode 100644
index 0000000..207df54
--- /dev/null
+++ b/Lib/various/plots/plot_general.m
@@ -0,0 +1,306 @@
+%% SIMULATION_GENERAL_V3
+% file: simulation_general_v3.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: function to setup and use the MHE out.observer on general model
+% INPUT: none
+% OUTPUT: params,out.obs
+% plot results for control design
+function plot_general(obj,varargin)
+
+    set(0,'DefaultFigureWindowStyle','docked');            
+    
+    fontsize = 20;
+    fig_count = 0;
+    
+    %%%% plot state estimation %%%
+    fig_count = fig_count+1;
+    figure(fig_count)            
+    sgtitle('State estimation')
+    for i=1:length(obj.setup.plot_vars)
+        subplot(length(obj.setup.plot_vars),1,i);
+        hold on
+        grid on
+        box on
+        
+        for traj=1:obj.setup.Ntraj
+            if 1 || strcmp(obj.setup.DataType,'simulated')
+                plot(obj.setup.time,obj.init.X(traj).val(obj.setup.plot_vars(i),:),'b--','LineWidth',2);
+            end
+            plot(obj.setup.time,obj.init.X_est_runtime(traj).val(obj.setup.plot_vars(i),:),'r--','LineWidth',2);                                                  
+        end    
+
+        % labels
+        set(gca,'fontsize', fontsize) 
+        ylabel(['x_',num2str(obj.setup.plot_vars(i))])
+    end
+    % legend
+    if strcat(obj.setup.DataType,'simulated')
+        legend('True','Est')
+    else
+        legend('Stored','Est','Runtime')
+    end    
+    xlabel(['time [s]'])
+    
+    
+    %%%% plot parameters estimation %%%
+    if ~isempty(obj.setup.plot_params)                
+        fig_count = fig_count+1;
+        figure(fig_count)
+        sgtitle('Parameters estimation')
+        for i=1:length(obj.setup.plot_params)
+            subplot(length(obj.setup.plot_params),1,i);
+            hold on
+            grid on
+            box on
+
+            for traj=1:obj.setup.Ntraj
+                if 1 || strcmp(obj.setup.DataType,'simulated')
+                    plot(obj.setup.time,obj.init.X(traj).val(obj.setup.plot_params(i),:),'b--','LineWidth',2);
+                end
+                plot(obj.setup.time,obj.init.X_est_runtime(traj).val(obj.setup.plot_params(i),:),'g--','LineWidth',2);                                                      
+            end
+
+            % labels
+            set(gca,'fontsize', fontsize)             
+            ylabel(['x_',num2str(obj.setup.plot_params(i))])
+        end
+
+        if strcat(obj.setup.DataType,'simulated')
+            legend('True','Est')
+        else
+            legend('Stored','Est','Runtime')
+        end
+        xlabel(['time [s]'])
+    end
+    
+    %%%% plot state estimation error %%%
+    if strcmp(obj.setup.DataType,'simulated')                
+        fig_count = fig_count+1;
+        figure(fig_count)
+        sgtitle('Estimation error - components')
+
+        for i=1:length(obj.setup.plot_vars)
+            subplot(length(obj.setup.plot_vars),1,i);
+            hold on
+            grid on
+            box on
+
+            % plot
+            est_error = obj.init.X(1).val(obj.setup.plot_vars(i),:) - obj.init.X_est_runtime(1).val(obj.setup.plot_vars(i),:);
+
+            log_flag = 1;
+            if ~log_flag
+                plot(obj.setup.time,est_error,'k','LineWidth',2);
+            else
+                % log 
+%                     set(gca, 'XScale', 'log')
+                set(gca, 'YScale', 'log')
+                plot(obj.setup.time,abs(est_error),'k','LineWidth',2);
+            end            
+
+            set(gca,'fontsize', fontsize)
+            ylabel(['\delta x_',num2str(obj.setup.plot_vars(i))])
+        end        
+        xlabel('time [s]')        
+    end
+    
+    %%%% plot parameters estimation error %%%
+    if 1 || strcmp(obj.setup.DataType,'simulated')
+        if ~isempty(obj.setup.plot_params)                    
+            fig_count = fig_count+1;
+            figure(fig_count)
+            sgtitle('Estimation error - parameters')
+
+            for i=1:length(obj.setup.plot_params)
+                subplot(length(obj.setup.plot_params),1,i);
+                hold on
+                grid on
+                box on
+
+                % plot
+                est_error = obj.init.X(1).val(obj.setup.plot_params(i),:) - obj.init.X_est_runtime(1).val(obj.setup.plot_params(i),:);
+
+                log_flag = 1;
+                if ~log_flag
+                    plot(obj.setup.time,est_error,'b','LineWidth',2);
+                else
+                    % log 
+%                     set(gca, 'XScale', 'log')
+                    set(gca, 'YScale', 'log')
+                    plot(obj.setup.time,abs(est_error),'b','LineWidth',2);
+                end
+
+                set(gca,'fontsize', fontsize)                
+                ylabel(['\delta x_',num2str(obj.setup.plot_params(i))])
+            end
+            xlabel('time [s]')
+        end
+    end
+    
+    %%%% plot state estimation error - norm%%%
+    if strcmp(obj.setup.DataType,'simulated')                
+        fig_count = fig_count+1;
+        figure(fig_count)
+        sgtitle('Estimation error state - norm')
+        hold on
+        grid on
+        box on
+
+        % plot
+        for iter=1:obj.setup.Niter
+            est_error_norm(iter) = norm(obj.init.X(1).val(obj.setup.plot_vars,iter) - obj.init.X_est_runtime(1).val(obj.setup.plot_vars,iter));
+        end
+
+        log_flag = 0;
+        if ~log_flag
+            plot(obj.setup.time,est_error_norm,'k','LineWidth',2);
+        else
+            % log 
+%                     set(gca, 'XScale', 'log')
+            set(gca, 'YScale', 'log')
+            plot(obj.setup.time,abs(est_error_norm),'r--','LineWidth',2);
+        end
+
+        set(gca,'fontsize', fontsize)
+        xlabel('time [s]')
+        ylabel('\delta x_norm') 
+    end
+    
+    %%%% plot params estimation error - norm%%%
+%     if 1 || strcmp(obj.setup.DataType,'simulated')                
+%         fig_count = fig_count+1;
+%         figure(fig_count)
+%         sgtitle('Estimation error params - norm')
+%         hold on
+%         grid on
+%         box on
+% 
+%         % plot
+%         for iter=1:obj.setup.Niter
+%             est_error_norm(iter) = norm(obj.init.X(1).val(obj.setup.plot_params,iter) - obj.init.X_est_runtime(1).val(obj.setup.plot_params,iter));
+%         end
+% 
+%         log_flag = 0;
+%         if ~log_flag
+%             plot(obj.setup.time,est_error_norm,'r','LineWidth',2);
+%         else
+%             % log 
+% %                     set(gca, 'XScale', 'log')
+%             set(gca, 'YScale', 'log')
+%             plot(obj.setup.time,abs(est_error_norm),'b--','LineWidth',2);
+%         end
+% 
+%         set(gca,'fontsize', fontsize)
+%         xlabel('time [s]')
+%         ylabel('\delta x_norm') 
+%     end
+
+    %%%% plot filters %%%%%            
+    fig_count = fig_count+1;
+    figure(fig_count)
+    sgtitle('Filters on measures')            
+    ax = zeros(1,3);
+    for k=1:obj.setup.J_nterm
+        
+        % number fo subplots depending on the Nterm
+        n_subplot = obj.setup.J_nterm;
+        
+        % indicize axes
+        ax_index = k;
+        ax(ax_index)=subplot(n_subplot,1,ax_index);                
+        
+        % plot
+        hold on
+        grid on
+        
+        for traj=1:obj.setup.Ntraj
+            for dim=obj.setup.dim_out_compare
+                y_plot = obj.setup.J_temp_scale(k)*reshape(obj.init.Y_full_story(traj).val(k,dim,:),size(obj.setup.time));
+                if strcmp(obj.setup.DataType,'simulated')
+                    ytrue_plot = obj.setup.J_temp_scale(k)*reshape(obj.init.Ytrue_full_story(traj).val(k,dim,:),size(obj.setup.time));
+                end
+                yhat_plot = obj.setup.J_temp_scale(k)*reshape(obj.init.Yhat_full_story(traj).val(k,dim,:),size(obj.setup.time));
+                if 1
+                    if strcmp(obj.setup.DataType,'simulated')
+                        plot(obj.setup.time,y_plot,'b--');
+                    end
+                    plot(obj.setup.time,yhat_plot,'r--','LineWidth',2);
+                    plot(obj.setup.time,y_plot,'k:','LineWidth',2);                            
+                else
+                    plot(obj.setup.time,abs(y_plot-yhat_plot));
+                    set(gca, 'YScale', 'log')
+                end
+            end                        
+        end    
+        if strcmp(obj.setup.DataType,'simulated')
+            legend('meas','estimation','target')
+        else
+            legend('meas','est')
+        end
+        
+        set(gca,'fontsize', fontsize)
+        ylabel(strcat('y_{filter}^',num2str(k)));
+        xlabel('simulation time [s]');
+        
+    end
+    linkaxes(ax,'x');
+    
+    
+    %%%% plot windowed data %%%%            
+    fig_count = fig_count+1;
+    figure(fig_count)
+    subplot(2,1,1)
+    grid on
+    sgtitle('Sampled measured')
+    ax = zeros(1,3);
+    for k=1:obj.setup.dim_out
+        
+        % number fo subplots depending on the output dimension
+        n_subplot = obj.setup.dim_out+1;
+        
+        % indicize axes
+        ax_index = k;
+        ax(ax_index)=subplot(n_subplot,1,ax_index);
+        
+        % hold on on plots
+        hold on
+        
+        % dpwn sampling instants
+        WindowTime = obj.setup.time(obj.init.temp_time);
+        
+        for traj=1:obj.setup.Ntraj
+            % plot true values
+            y_meas = reshape(obj.init.Y_full_story(traj).val(1,k,:),size(obj.setup.time));
+            plot(obj.setup.time,y_meas,'m:','LineWidth',2)
+
+            % plot target values    
+            try
+                data = reshape(obj.init.target_story(traj).val(1,k,obj.init.temp_time),1,length(WindowTime));
+                plot(WindowTime,data,'bo','MarkerSize',5);
+            catch 
+                disp('CHECK T_END OR AYELS CONDITION - LOOKS LIKE NO OPTIMISATION HAS BEEN RUN')
+            end            
+        end
+        set(gca,'fontsize', fontsize)
+        ylabel(strcat('y_',num2str(k)));        
+    end
+    xlabel('simulation time [s]');
+    legend('meas','sampled')
+    linkaxes(ax(1:n_subplot-1),'x');
+    %%% plot adaptive sampling            
+    ax(n_subplot) = subplot(n_subplot,1,n_subplot);
+    % frequency constraint
+    % y_meas = squeeze(obj.init.Y_full_story.val(1,1,:));  
+    % [WT,F] = cwt(y_meas,obj.init.wvname,1/obj.setup.Ts,'VoicesPerOctave',obj.init.Nv,'FrequencyLimits',obj.init.FLIMITS);    
+    % heatmap(obj.setup.time,F,real(WT))
+    % grid off
+    % colormap jet
+    % 
+    % %%% single cwt
+    % fig_count = fig_count+1;
+    % figure(fig_count)
+    % cwt(y_meas,obj.init.wvname,1/obj.setup.Ts,'VoicesPerOctave',obj.init.Nv,'FrequencyLimits',obj.init.FLIMITS);
+            
+    
+end
\ No newline at end of file
diff --git a/Lib/various/plots/plot_general_calce_fede.m b/Lib/various/plots/plot_general_calce_fede.m
new file mode 100644
index 0000000..6f6e7a7
--- /dev/null
+++ b/Lib/various/plots/plot_general_calce_fede.m
@@ -0,0 +1,177 @@
+%% SIMULATION_GENERAL_V3
+% file: simulation_general_v3.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: function to setup and use the MHE out.observer on general model
+% INPUT: none
+% OUTPUT: params,out.obs
+% plot results for control design
+function plot_general_calce_fede(obj,varargin)
+    
+    set(0,'DefaultFigureWindowStyle','docked');            
+    
+    fontsize = 20;
+    fig_count = 0;
+    category20 = [
+    31, 119, 180; % Color 1: Blue
+    255, 127, 14; % Color 2: Orange
+    44, 160, 44; % Color 3: Green
+    214, 39, 40; % Color 4: Red
+    148, 103, 189; % Color 5: Purple
+    140, 86, 75; % Color 6: Brown
+    227, 119, 194; % Color 7: Pink
+    127, 127, 127; % Color 8: Gray
+    188, 189, 34; % Color 9: Olive
+    23, 190, 207; % Color 10: Cyan
+    174, 199, 232; % Color 11: Light Blue
+    255, 187, 120; % Color 12: Light Orange
+    152, 223, 138; % Color 13: Light Green
+    255, 152, 150; % Color 14: Light Red
+    197, 176, 213; % Color 15: Lavender
+    196, 156, 148; % Color 16: Tan
+    247, 182, 210; % Color 17: Light Pink
+    199, 199, 199; % Color 18: Light Gray
+    219, 219, 141; % Color 19: Light Olive
+    158, 218, 229  % Color 20: Light Cyan
+] / 255; 
+    
+    %%%% plot state estimation %%%
+    fig_count = fig_count+1;
+    figure(fig_count)            
+    sgtitle('State estimation')
+    ylabels = {'$Z$', '$V1$', '$OCV$'};
+    for i=1:length(obj.setup.plot_vars)
+        subplot(length(obj.setup.plot_vars),1,i);
+        hold on
+        grid on
+        box on
+
+        true_values = [];
+        estimated_values = [];
+        
+        for traj = 1:obj.setup.Ntraj
+            if 1 || strcmp(obj.setup.DataType,'simulated')
+                plot(obj.setup.time, obj.init.X(traj).val(obj.setup.plot_vars(i), :), 'LineStyle', '--', 'LineWidth', 2, 'Color', 'blue');
+                true_values = [true_values; obj.init.X(traj).val(obj.setup.plot_vars(i), :)];
+            end
+            plot(obj.setup.time, obj.init.X_est_runtime(traj).val(obj.setup.plot_vars(i), :), 'LineStyle', '--', 'LineWidth', 2, 'Color', category20(2, :));
+            estimated_values = [estimated_values; obj.init.X_est_runtime(traj).val(obj.setup.plot_vars(i), :)];
+        end    
+
+        % labels
+        set(gca,'fontsize', fontsize) 
+        ylabel(ylabels{i},'Interpreter','latex');
+    end
+    % legend
+    if strcat(obj.setup.DataType,'simulated')
+        legend('True','Est')
+    else
+        legend('Stored','Est','Runtime')
+    end    
+    xlabel('time [s]')   
+    
+    %%%% plot parameters estimation %%%
+    if ~isempty(obj.setup.plot_params)
+    
+        % Define groups of parameters
+        groups = {
+            [4, 5, 6]... 
+            [7, 11, 15, 19]... 
+            [8, 12, 16, 20]... 
+            [9, 13, 17, 21]... 
+            [10, 14, 18, 22]
+        };
+        ylabels = {
+                   ['$R0$'; '$R1$'; '$C1$'], ...
+                   ['$\alpha_{0,OCV}$'; '$\alpha_{1,OCV}$'; '$\alpha_{2,OCV}$'; '$\alpha_{3,OCV}$'], ...
+                   ['$\alpha_{0,R0}$'; '$\alpha_{1,R0}$'; '$\alpha_{2,R0}$'; '$\alpha_{3,R0}$'], ...
+                   ['$\alpha_{0,R1}$'; '$\alpha_{1,R1}$'; '$\alpha_{2,R1}$'; '$\alpha_{3,R1}$'], ...
+                   ['$\alpha_{0,C1}$'; '$\alpha_{1,C1}$'; '$\alpha_{2,C1}$'; '$\alpha_{3,C1}$']
+                   };        
+
+        % Iterate over each group
+        for g = 1:length(groups)
+            fig_count = fig_count + 1;
+            figure(fig_count)
+            sgtitle('Parameters estimation')
+    
+            % Plot each parameter in the group
+            for i = 1:length(groups{g})
+                subplot(length(groups{g}), 1, i);
+                hold on
+                grid on
+                box on
+    
+                for traj = 1:obj.setup.Ntraj
+                    if 1 || strcmp(obj.setup.DataType, 'simulated')
+                        plot(obj.setup.time, obj.init.X(traj).val(groups{g}(i), :), 'b--', 'LineWidth', 2);
+                    end
+                    plot(obj.setup.time, obj.init.X_est_runtime(traj).val(groups{g}(i), :), 'LineStyle', '--', 'LineWidth', 2, 'Color', category20(2, :));
+                end
+    
+                % labelsval
+                set(gca, 'fontsize', fontsize)
+                ylabel(ylabels{g}(i,:), 'Interpreter', 'latex');
+            end                
+            xlabel(['time [s]'])
+        end
+    end                   
+    
+    %%%% plot windowed data %%%%            
+    fig_count = fig_count+1;
+    figure(fig_count)
+    subplot(2,1,1)
+    grid on
+    sgtitle('Sampled measured')
+    ax = zeros(1,3);
+    for k=1:obj.setup.dim_out
+        
+        % number fo subplots depending on the output dimension
+        n_subplot = obj.setup.dim_out+1;
+        
+        % indicize axes
+        ax_index = k;
+        ax(ax_index)=subplot(n_subplot,1,ax_index);
+        
+        % hold on on plots
+        hold on
+        
+        % dpwn sampling instants
+        WindowTime = obj.setup.time(obj.init.temp_time);
+        
+        for traj=1:obj.setup.Ntraj
+            % plot true values
+            y_meas = reshape(obj.init.Y_full_story(traj).val(1,k,:),size(obj.setup.time));
+            plot(obj.setup.time,y_meas,'m:','LineWidth',2)
+
+            % plot target values    
+            try
+                data = reshape(obj.init.target_story(traj).val(1,k,obj.init.temp_time),1,length(WindowTime));
+                plot(WindowTime,data,'bo','MarkerSize',5);
+            catch 
+                disp('CHECK T_END OR AYELS CONDITION - LOOKS LIKE NO OPTIMISATION HAS BEEN RUN')
+            end            
+        end
+        set(gca,'fontsize', fontsize)
+        ylabel(strcat('y_',num2str(k)));        
+    end
+    xlabel('simulation time [s]');
+    legend('meas','sampled')
+    linkaxes(ax(1:n_subplot-1),'x');
+    
+    %%% plot adaptive sampling            
+    ax(n_subplot) = subplot(n_subplot,1,n_subplot);
+    % frequency constraint
+    % y_meas = squeeze(obj.init.Y_full_story.val(1,1,:));  
+    % [WT,F] = cwt(y_meas,obj.init.wvname,1/obj.setup.Ts,'VoicesPerOctave',obj.init.Nv,'FrequencyLimits',obj.init.FLIMITS);    
+    % heatmap(obj.setup.time,F,real(WT))
+    % grid off
+    % colormap jet
+    % 
+    % %%% single cwt
+    fig_count = fig_count+1;
+    figure(fig_count)
+    cwt(y_meas,obj.init.wvname,1/obj.setup.Ts,'VoicesPerOctave',obj.init.Nv,'FrequencyLimits',obj.init.FLIMITS);
+            
+    
+end
\ No newline at end of file
diff --git a/Lib/various/plots/plot_general_calce_tushar.m b/Lib/various/plots/plot_general_calce_tushar.m
new file mode 100644
index 0000000..76a43b4
--- /dev/null
+++ b/Lib/various/plots/plot_general_calce_tushar.m
@@ -0,0 +1,427 @@
+%% SIMULATION_GENERAL_V3
+% file: simulation_general_v3.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: function to setup and use the MHE out.observer on general model
+% INPUT: none
+% OUTPUT: params,out.obs
+% plot results for control design
+function plot_general_calce_tushar(obj,varargin)
+    
+    set(0,'DefaultFigureWindowStyle','docked');            
+    
+    fontsize = 20;
+    fig_count = 0;
+    category20 = [
+    31, 119, 180; % Color 1: Blue
+    255, 127, 14; % Color 2: Orange
+    44, 160, 44; % Color 3: Green
+    214, 39, 40; % Color 4: Red
+    148, 103, 189; % Color 5: Purple
+    140, 86, 75; % Color 6: Brown
+    227, 119, 194; % Color 7: Pink
+    127, 127, 127; % Color 8: Gray
+    188, 189, 34; % Color 9: Olive
+    23, 190, 207; % Color 10: Cyan
+    174, 199, 232; % Color 11: Light Blue
+    255, 187, 120; % Color 12: Light Orange
+    152, 223, 138; % Color 13: Light Green
+    255, 152, 150; % Color 14: Light Red
+    197, 176, 213; % Color 15: Lavender
+    196, 156, 148; % Color 16: Tan
+    247, 182, 210; % Color 17: Light Pink
+    199, 199, 199; % Color 18: Light Gray
+    219, 219, 141; % Color 19: Light Olive
+    158, 218, 229  % Color 20: Light Cyan
+] / 255; 
+    
+    %%%% plot state estimation %%%
+    for i=1:length(obj.setup.plot_vars)
+        fig_count = fig_count+1;
+        figure(fig_count)
+
+        % First subplot: True and estimated values
+        subplot(2, 1, 1);
+        hold on;
+        grid on;
+        box on;
+        true_values = [];
+        estimated_values = [];
+        
+        for traj = 1:obj.setup.Ntraj
+            if 1 || strcmp(obj.setup.DataType,'simulated')
+                plot(obj.setup.time, obj.init.X(traj).val(obj.setup.plot_vars(i), :), 'LineStyle', '--', 'LineWidth', 2, 'Color', 'blue');
+                true_values = [true_values; obj.init.X(traj).val(obj.setup.plot_vars(i), :)];
+            end
+            plot(obj.setup.time, obj.init.X_est_runtime(traj).val(obj.setup.plot_vars(i), :), 'LineStyle', '--', 'LineWidth', 2, 'Color', category20(2, :));
+            estimated_values = [estimated_values; obj.init.X_est_runtime(traj).val(obj.setup.plot_vars(i), :)];
+        end    
+
+        % Labels
+        set(gca, 'fontsize', fontsize);
+        ylabel(['x_', num2str(obj.setup.plot_vars(i))]);
+        
+        xlabel('time[s]');
+        legend('True', 'Est');
+
+
+        % Second subplot: Error
+        subplot(2, 1, 2);
+        hold on;
+        grid on;
+        box on;
+
+        for traj = 1:obj.setup.Ntraj
+            error = obj.init.X(traj).val(obj.setup.plot_vars(i), :) - obj.init.X_est_runtime(traj).val(obj.setup.plot_vars(i), :);
+            plot(obj.setup.time, error, 'LineStyle', '--', 'LineWidth', 2, 'Color', category20(4, :));
+        end
+
+        % Calculate Metrics
+
+        if i==1
+            errors = true_values(obj.init.Nw_Nts:end) - estimated_values(obj.init.Nw_Nts:end);
+            RMSE = sqrt(mean(errors.^2));
+            R2 = 1 - sum(errors.^2) / sum((true_values(obj.init.Nw_Nts:end) - mean(true_values(obj.init.Nw_Nts:end))).^2);
+            MAPE = mean(abs(errors ./ true_values(obj.init.Nw_Nts:end))) * 100;
+    
+            % Display Metrics
+            metrics_text = {
+                ['SOC RMSE: ', num2str(RMSE)];
+                ['SOC R²: ', num2str(R2)];
+                ['SOC MAPE: ', num2str(MAPE), '%']
+            };
+            % dim = [.15 .5 .3 .3];
+            % annotation('textbox', dim, 'String', metrics_text, 'FitBoxToText', 'on', 'BackgroundColor', 'white');
+            disp(metrics_text)
+        end
+
+        % Labels
+        set(gca, 'fontsize', fontsize);
+        ylabel(['Error in x_', num2str(obj.setup.plot_vars(i))]);
+        xlabel('time [s]');
+        title('Estimation Error');
+     
+
+    end    
+    
+    %%%% plot parameters estimation %%%
+    if ~isempty(obj.setup.plot_params)
+    
+        % Define groups of parameters
+        groups = {
+            [4, 5, 6]... 
+            [7, 11, 15, 19]... 
+            [8, 12, 16, 20]... 
+            [9, 13, 17, 21]... 
+            [10, 14, 18, 22]
+        };
+        ylabels = {
+                   ['$R0$'; '$R1$'; '$C1$'], ...
+                   ['$\alpha_{0,OCV}$'; '$\alpha_{1,OCV}$'; '$\alpha_{2,OCV}$'; '$\alpha_{3,OCV}$'], ...
+                   ['$\alpha_{0,R0}$'; '$\alpha_{1,R0}$'; '$\alpha_{2,R0}$'; '$\alpha_{3,R0}$'], ...
+                   ['$\alpha_{0,R1}$'; '$\alpha_{1,R1}$'; '$\alpha_{2,R1}$'; '$\alpha_{3,R1}$'], ...
+                   ['$\alpha_{0,C1}$'; '$\alpha_{1,C1}$'; '$\alpha_{2,C1}$'; '$\alpha_{3,C1}$']
+                   };        
+
+        % Iterate over each group
+        for g = 1:length(groups)
+            fig_count = fig_count + 1;
+            sgtitle('Parameters estimation')
+    
+            % Plot each parameter in the group
+            for i = 1:length(groups{g})
+                subplot(length(groups{g}), 1, i);
+                hold on
+                grid on
+                box on
+    
+                for traj = 1:obj.setup.Ntraj
+                    % if 1 || strcmp(obj.setup.DataType, 'simulated')
+                    %     plot(obj.setup.time, obj.init.X(traj).val(groups{g}(i), :), 'b--', 'LineWidth', 2);
+                    % end
+                    plot(obj.setup.time, obj.init.X_est_runtime(traj).val(groups{g}(i), :), 'LineStyle', '--', 'LineWidth', 2, 'Color', category20(2, :));
+                end
+    
+                % labelsval
+                set(gca, 'fontsize', fontsize)
+                ylabel(ylabels{g}(i,:), 'Interpreter', 'latex');
+                end
+    
+            % if strcmp(obj.setup.DataType, 'simulated')
+            %     legend('True', 'Est')
+            % else
+            %     legend('Stored', 'Est', 'Runtime')
+            % end
+            xlabel(['time [s]'])
+        end
+    end
+
+    %%%% plot parameters estimation %%%
+    % if ~isempty(obj.setup.plot_params)
+    % 
+    %     fig_count = fig_count+1;
+    %     figure(fig_count)
+    %     sgtitle('Parameters estimation')
+    %     for i=1:length(obj.setup.plot_params)
+    %         subplot(length(obj.setup.plot_params),1,i);
+    %         hold on
+    %         grid on
+    %         box on
+    % 
+    %         for traj=1:obj.setup.Ntraj
+    %             if 1 || strcmp(obj.setup.DataType,'simulated')
+    %                 plot(obj.setup.time,obj.init.X(traj).val(obj.setup.plot_params(i),:),'b--','LineWidth',2);
+    %             end
+    %             plot(obj.setup.time,obj.init.X_est_runtime(traj).val(obj.setup.plot_params(i),:),'g--','LineWidth',2);                                                      
+    %         end
+    % 
+    %         % labels
+    %         set(gca,'fontsize', fontsize)             
+    %         ylabel(['x_',num2str(obj.setup.plot_params(i))])
+    %     end
+    % 
+    %     if strcat(obj.setup.DataType,'simulated')
+    %         legend('True','Est')
+    %     else
+    %         legend('Stored','Est','Runtime')
+    %     end
+    %     xlabel(['time [s]'])
+    % end
+    
+%     %%%% plot state estimation error %%%
+%     if strcmp(obj.setup.DataType,'simulated')                
+%         fig_count = fig_count+1;
+%         figure(fig_count)
+%         sgtitle('Estimation error - components')
+% 
+%         for i=1:length(obj.setup.plot_vars)
+%             subplot(length(obj.setup.plot_vars),1,i);
+%             hold on
+%             grid on
+%             box on
+% 
+%             % plot
+%             est_error = obj.init.X(1).val(obj.setup.plot_vars(i),:) - obj.init.X_est_runtime(1).val(obj.setup.plot_vars(i),:);
+% 
+%             log_flag = 1;
+%             if ~log_flag
+%                 plot(obj.setup.time,est_error,'k','LineWidth',2);
+%             else
+%                 % log 
+% %                     set(gca, 'XScale', 'log')
+%                 set(gca, 'YScale', 'log')
+%                 plot(obj.setup.time,abs(est_error),'k','LineWidth',2);
+%             end            
+% 
+%             set(gca,'fontsize', fontsize)
+%             ylabel(['\delta x_',num2str(obj.setup.plot_vars(i))])
+%         end        
+%         xlabel('time [s]')        
+%     end
+% 
+%     %%%% plot parameters estimation error %%%
+%     if 1 || strcmp(obj.setup.DataType,'simulated')
+%         if ~isempty(obj.setup.plot_params)                    
+%             fig_count = fig_count+1;
+%             figure(fig_count)
+%             sgtitle('Estimation error - parameters')
+% 
+%             for i=1:length(obj.setup.plot_params)
+%                 subplot(length(obj.setup.plot_params),1,i);
+%                 hold on
+%                 grid on
+%                 box on
+% 
+%                 % plot
+%                 est_error = obj.init.X(1).val(obj.setup.plot_params(i),:) - obj.init.X_est_runtime(1).val(obj.setup.plot_params(i),:);
+% 
+%                 log_flag = 1;
+%                 if ~log_flag
+%                     plot(obj.setup.time,est_error,'b','LineWidth',2);
+%                 else
+%                     % log 
+% %                     set(gca, 'XScale', 'log')
+%                     set(gca, 'YScale', 'log')
+%                     plot(obj.setup.time,abs(est_error),'b','LineWidth',2);
+%                 end
+% 
+%                 set(gca,'fontsize', fontsize)                
+%                 ylabel(['\delta x_',num2str(obj.setup.plot_params(i))])
+%             end
+%             xlabel('time [s]')
+%         end
+%     end
+    
+    %%%% plot state estimation error - norm%%%
+    if strcmp(obj.setup.DataType,'simulated')                
+        fig_count = fig_count+1;
+        figure(fig_count)
+        sgtitle('Estimation error state - norm')
+        hold on
+        grid on
+        box on
+
+        % plot
+        for iter=1:obj.setup.Niter
+            est_error_norm(iter) = norm(obj.init.X(1).val(obj.setup.plot_vars,iter) - obj.init.X_est_runtime(1).val(obj.setup.plot_vars,iter));
+        end
+
+        log_flag = 0;
+        if ~log_flag
+            plot(obj.setup.time,est_error_norm,'k','LineWidth',2);
+        else
+            % log 
+%                     set(gca, 'XScale', 'log')
+            set(gca, 'YScale', 'log')
+            plot(obj.setup.time,abs(est_error_norm),'r--','LineWidth',2);
+        end
+
+        set(gca,'fontsize', fontsize)
+        xlabel('time [s]')
+        ylabel('\delta x_norm') 
+    end
+    
+    %%%% plot params estimation error - norm%%%
+    if 1 || strcmp(obj.setup.DataType,'simulated')                
+        fig_count = fig_count+1;
+        figure(fig_count)
+        sgtitle('Estimation error params - norm')
+        hold on
+        grid on
+        box on
+
+        % plot
+        for iter=1:obj.setup.Niter
+            est_error_norm(iter) = norm(obj.init.X(1).val(obj.setup.plot_params,iter) - obj.init.X_est_runtime(1).val(obj.setup.plot_params,iter));
+        end
+
+        log_flag = 0;
+        if ~log_flag
+            plot(obj.setup.time,est_error_norm,'r','LineWidth',2);
+        else
+            % log 
+%                     set(gca, 'XScale', 'log')
+            set(gca, 'YScale', 'log')
+            plot(obj.setup.time,abs(est_error_norm),'b--','LineWidth',2);
+        end
+
+        set(gca,'fontsize', fontsize)
+        xlabel('time [s]')
+        ylabel('\delta x_norm') 
+    end
+
+    %%%% plot filters %%%%%            
+    fig_count = fig_count+1;
+    figure(fig_count)
+    sgtitle('Filters on measures')            
+    ax = zeros(1,3);
+    for k=1:obj.setup.J_nterm
+        
+        % number fo subplots depending on the Nterm
+        n_subplot = obj.setup.J_nterm;
+        
+        % indicize axes
+        ax_index = k;
+        ax(ax_index)=subplot(n_subplot,1,ax_index);                
+        
+        % plot
+        hold on
+        grid on
+        
+        for traj=1:obj.setup.Ntraj
+            for dim=obj.setup.dim_out_compare
+                y_plot = obj.setup.J_temp_scale(k)*reshape(obj.init.Y_full_story(traj).val(k,dim,:),size(obj.setup.time));
+                if strcmp(obj.setup.DataType,'simulated')
+                    ytrue_plot = obj.setup.J_temp_scale(k)*reshape(obj.init.Ytrue_full_story(traj).val(k,dim,:),size(obj.setup.time));
+                end
+                yhat_plot = obj.setup.J_temp_scale(k)*reshape(obj.init.Yhat_full_story(traj).val(k,dim,:),size(obj.setup.time));
+
+                if strcmp(obj.setup.DataType,'simulated')
+                    plot(obj.setup.time,y_plot,'b--');
+                end
+                plot(obj.setup.time,yhat_plot,'LineStyle', ':', 'LineWidth', 2, 'Color', category20(2, :));
+                plot(obj.setup.time,y_plot,'LineStyle', '--', 'LineWidth', 2, 'Color', 'blue');                            
+
+            end                        
+        end    
+        legend('est','meas')
+        set(gca,'fontsize', fontsize)
+        ylabel('voltage [V]');
+        xlabel('time [s]');
+
+        errors = y_plot(obj.init.Nw_Nts:end) - yhat_plot(obj.init.Nw_Nts:end);
+        RMSE = sqrt(mean(errors.^2));
+        R2 = 1 - sum(errors.^2) / sum((y_plot(obj.init.Nw_Nts:end) - mean(y_plot(obj.init.Nw_Nts:end))).^2);
+        MAPE = mean(abs(errors ./ y_plot(obj.init.Nw_Nts:end))) * 100;
+
+        % Display Metrics
+        metrics_text = {
+            ['Voltage RMSE: ', num2str(RMSE)];
+            ['Voltage R²: ', num2str(R2)];
+            ['Voltage MAPE: ', num2str(MAPE), '%']
+        };
+        % dim = [.15 .5 .3 .3];
+        % annotation('textbox', dim, 'String', metrics_text, 'FitBoxToText', 'on', 'BackgroundColor', 'white');
+        disp(metrics_text)
+        
+    end
+    linkaxes(ax,'x');  
+    
+    %%%% plot windowed data %%%%            
+    fig_count = fig_count+1;
+    figure(fig_count)
+    subplot(2,1,1)
+    grid on
+    sgtitle('Sampled measured')
+    ax = zeros(1,3);
+    for k=1:obj.setup.dim_out
+        
+        % number fo subplots depending on the output dimension
+        n_subplot = obj.setup.dim_out+1;
+        
+        % indicize axes
+        ax_index = k;
+        ax(ax_index)=subplot(n_subplot,1,ax_index);
+        
+        % hold on on plots
+        hold on
+        
+        % dpwn sampling instants
+        WindowTime = obj.setup.time(obj.init.temp_time);
+        
+        for traj=1:obj.setup.Ntraj
+            % plot true values
+            y_meas = reshape(obj.init.Y_full_story(traj).val(1,k,:),size(obj.setup.time));
+            plot(obj.setup.time,y_meas,'m:','LineWidth',2)
+
+            % plot target values    
+            try
+                data = reshape(obj.init.target_story(traj).val(1,k,obj.init.temp_time),1,length(WindowTime));
+                plot(WindowTime,data,'bo','MarkerSize',5);
+            catch 
+                disp('CHECK T_END OR AYELS CONDITION - LOOKS LIKE NO OPTIMISATION HAS BEEN RUN')
+            end            
+        end
+        set(gca,'fontsize', fontsize)
+        ylabel(strcat('y_',num2str(k)));        
+    end
+    xlabel('simulation time [s]');
+    legend('meas','sampled')
+    linkaxes(ax(1:n_subplot-1),'x');
+    
+    %%% plot adaptive sampling            
+    ax(n_subplot) = subplot(n_subplot,1,n_subplot);
+    % frequency constraint
+    % y_meas = squeeze(obj.init.Y_full_story.val(1,1,:));  
+    % [WT,F] = cwt(y_meas,obj.init.wvname,1/obj.setup.Ts,'VoicesPerOctave',obj.init.Nv,'FrequencyLimits',obj.init.FLIMITS);    
+    % heatmap(obj.setup.time,F,real(WT))
+    % grid off
+    % colormap jet
+    % 
+    % %%% single cwt
+    fig_count = fig_count+1;
+    figure(fig_count)
+    cwt(y_meas,obj.init.wvname,1/obj.setup.Ts,'VoicesPerOctave',obj.init.Nv,'FrequencyLimits',obj.init.FLIMITS);
+            
+    
+end
\ No newline at end of file
diff --git a/Lib/various/plots/plot_interp.m b/Lib/various/plots/plot_interp.m
new file mode 100644
index 0000000..bf951b6
--- /dev/null
+++ b/Lib/various/plots/plot_interp.m
@@ -0,0 +1,41 @@
+%% function 
+function plot_interp(obs,str,pos,params,fig,sim,range)
+
+    
+
+    % create data
+    if ~sim
+        data = obs.init.cloud_X(1,:);
+        % sort in ascending order
+        [data_s, idx_s] = sort(data);
+    else
+        data_s = obs.init.X_est.val(1,:);
+    end
+    
+    
+    % compute poly
+    for p=1:length(params)
+       data_p(p,:) = data_s.^(p-1); 
+    end
+
+    % plot
+    figure(fig)
+    hold on
+    grid on
+    box on
+    
+    if ~sim
+        plot(obs.init.params.input_soc,str,'p','LineWidth',2,'MarkerSize',20)  
+        plot(data_s,obs.init.cloud_Y(pos,idx_s),'r+','LineWidth',2)
+        
+        % plot estimation
+        plot(data_s,params'*data_p,'m--o','LineWidth',2) 
+    else        
+        % plot estimation
+        plot(data_s,params'*data_p,'b--o','LineWidth',2)            
+    end
+
+    
+
+
+end
\ No newline at end of file
diff --git a/Lib/various/plots/plot_poly_model.m b/Lib/various/plots/plot_poly_model.m
new file mode 100644
index 0000000..d7e4a44
--- /dev/null
+++ b/Lib/various/plots/plot_poly_model.m
@@ -0,0 +1,81 @@
+%%
+function plot_poly_model(obs_fast,init_flag,paramCloud,final_flag)
+
+    fontsize=20;        
+
+    % poly model
+    if init_flag   
+        Shift = (obs_fast.setup.w+1)*obs_fast.setup.Nts;
+        for iter=Shift:obs_fast.setup.Nts:obs_fast.setup.Niter
+            Zdiff = obs_fast.init.X_est_runtime.val(1,iter)-sort(obs_fast.init.cloud_X(1,:));
+            pos = find(abs(Zdiff) == min(abs(Zdiff)),1,'first'); 
+            scale = 40;
+            range = max(1,pos-ceil(size(obs_fast.init.cloud_X,2)/scale)):max(size(obs_fast.init.cloud_X,2),pos+ceil(size(obs_fast.init.cloud_X,2)/scale));
+            try
+                plot_interp(obs_fast,obs_fast.init.params.input_OCV,1,obs_fast.init.X_est_runtime.val([7 11 15 19],iter),1,1,range);
+                plot_interp(obs_fast,obs_fast.init.params.input_R0,2,obs_fast.init.X_est_runtime.val([8 12 16 20],iter),2,1,range);
+                plot_interp(obs_fast,obs_fast.init.params.input_R1,3,obs_fast.init.X_est_runtime.val([9 13 17 21],iter),3,1,range);
+                plot_interp(obs_fast,obs_fast.init.params.input_C1,4,obs_fast.init.X_est_runtime.val([10 14 18 22],iter),4,1,range);
+            catch
+               ARARMAX = 1; 
+            end
+        end       
+    end
+    
+    % initial guess
+    iter = 1;
+    plot_interp(obs_fast,obs_fast.setup.params.input_OCV,1,obs_fast.init.X_est_runtime.val([7 11 15 19],iter),1,0,0);
+    plot_interp(obs_fast,obs_fast.setup.params.input_R0,2,obs_fast.init.X_est_runtime.val([8 12 16 20],iter),2,0,0);
+    plot_interp(obs_fast,obs_fast.setup.params.input_R1,3,obs_fast.init.X_est_runtime.val([9 13 17 21],iter),3,0,0);
+    plot_interp(obs_fast,obs_fast.setup.params.input_C1,4,obs_fast.init.X_est_runtime.val([10 14 18 22],iter),4,0,0);
+    
+    if final_flag
+        iter = obs_fast.setup.Niter;
+        range = 1:obs_fast.setup.Niter;
+        plot_interp(obs_fast,obs_fast.init.params.input_OCV,1,obs_fast.init.X_est.val([7 11 15 19],iter),1,0,range);
+        plot_interp(obs_fast,obs_fast.init.params.input_R0,2,obs_fast.init.X_est.val([8 12 16 20],iter),2,0,range);
+        plot_interp(obs_fast,obs_fast.init.params.input_R1,3,obs_fast.init.X_est.val([9 13 17 21],iter),3,0,range);
+        plot_interp(obs_fast,obs_fast.init.params.input_C1,4,obs_fast.init.X_est.val([10 14 18 22],iter),4,0,range);        
+    end
+    
+    % param cloud
+    if paramCloud
+        figure(1)
+        set(gca,'fontsize', fontsize)
+        grid on
+        hold on
+        plot(obs_fast.init.params.input_soc,obs_fast.setup.params.input_data.OCV,'LineWidth',2)
+        plot(obs_fast.init.params.input_soc,obs_fast.setup.params.input_data.OCV_nominal,'LineWidth',2)
+        plot(obs_fast.init.X_est.val(1,:),obs_fast.init.X_est.val(3,:),'co');
+        legend('measurements','fit data','initial fit','nominal','real','estimated');
+        
+        figure(2)
+        set(gca,'fontsize', fontsize)
+        grid on
+        hold on
+        plot(obs_fast.init.params.input_soc,obs_fast.setup.params.input_data.R0,'LineWidth',2)
+        plot(obs_fast.init.params.input_soc,obs_fast.setup.params.input_data.R0_nominal,'LineWidth',2)
+        plot(obs_fast.init.X_est.val(1,:),obs_fast.init.X_est.val(4,:),'co');
+        legend('measurements','fit data','initial fit','nominal','real','estimated');
+        
+        figure(3)
+        set(gca,'fontsize', fontsize)
+        grid on
+        hold on
+        plot(obs_fast.init.params.input_soc,obs_fast.setup.params.input_data.R1,'LineWidth',2)
+        plot(obs_fast.init.params.input_soc,obs_fast.setup.params.input_data.R1_nominal,'LineWidth',2)
+        plot(obs_fast.init.X_est.val(1,:),obs_fast.init.X_est.val(5,:),'co');
+        legend('measurements','fit data','initial fit','nominal','real','estimated');
+        
+        figure(4)
+        set(gca,'fontsize', fontsize)
+        grid on
+        hold on
+        plot(obs_fast.init.params.input_soc,obs_fast.setup.params.input_data.C1,'LineWidth',2)
+        plot(obs_fast.init.params.input_soc,obs_fast.setup.params.input_data.C1_nominal,'LineWidth',2)
+        plot(obs_fast.init.X_est.val(1,:),obs_fast.init.X_est.val(6,:),'co');
+        legend('measurements','fit data','initial fit','nominal','real','estimated');
+    end
+   
+
+end
\ No newline at end of file
diff --git a/Lib/various/plots/plot_rover.m b/Lib/various/plots/plot_rover.m
new file mode 100644
index 0000000..e4696e9
--- /dev/null
+++ b/Lib/various/plots/plot_rover.m
@@ -0,0 +1,159 @@
+%% SIMULATION_GENERAL_V3
+% file: simulation_general_v3.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: function to setup and use the MHE out.observer on general model
+% INPUT: none
+% OUTPUT: params,out.obs
+% plot results for control design
+function plot_rover(obj,varargin)
+    
+    set(0,'DefaultFigureWindowStyle','docked');            
+    
+    fontsize = 20;
+    fig_count = 0;
+    
+    if length(varargin) == 1
+        params = varargin{1};
+    end
+    
+    %%%% plot state estimation %%%
+    fig_count = fig_count+1;
+    figure(fig_count)            
+    sgtitle('State estimation')
+    for i=1:length(obj.setup.plot_vars)
+        subplot(length(obj.setup.plot_vars),1,i);
+        hold on
+        grid on
+        box on
+        
+        for traj=1:obj.setup.Ntraj            
+            plot(obj.setup.time,obj.init.X(traj).val(obj.setup.plot_vars(i),:),'--','LineWidth',2);
+            set(gca,'ColorOrderIndex',traj)
+            plot(obj.setup.time,obj.init.X_est(traj).val(obj.setup.plot_vars(i),:),'LineWidth',2);                                                             
+            legend('True','Est')            
+        end
+        
+        % labels
+        set(gca,'fontsize', fontsize)         
+        ylabel(['x_',num2str(obj.setup.plot_vars(i))])
+    end
+    xlabel(['time [s]'])
+    
+    %%%% plot parameters estimation %%%
+    if ~isempty(obj.setup.plot_params)                
+        fig_count = fig_count+1;
+        figure(fig_count)
+        sgtitle('Parameters estimation')
+        for i=1:length(obj.setup.plot_params)
+            subplot(length(obj.setup.plot_params),1,i);
+            hold on
+            grid on
+            box on
+
+            for traj=1:obj.setup.Ntraj
+                plot(obj.setup.time,obj.init.X(traj).val(obj.setup.plot_params(i),:),'--','LineWidth',2);                
+                set(gca,'ColorOrderIndex',traj)
+                plot(obj.setup.time,obj.init.X_est(traj).val(obj.setup.plot_params(i),:),'LineWidth',2);                                                      
+            end
+
+            % labels
+            set(gca,'fontsize', fontsize)             
+            ylabel(['x_',num2str(obj.setup.plot_params(i))])
+        end
+        xlabel(['time [s]'])
+        legend('True','Est')                
+    end      
+    
+    
+    %%%% plot windowed data %%%%            
+    fig_count = fig_count+1;
+    figure(fig_count)
+    subplot(2,1,1)    
+    sgtitle('Sampled measured')
+    ax = zeros(1,3);
+    for k=1:length(obj.setup.params.dim_out_plot)
+        
+        % number fo subplots depending on the output dimension
+        n_subplot = length(obj.setup.params.dim_out_plot);
+        
+        % indicize axes
+        ax_index = k;
+        ax(ax_index)=subplot(n_subplot,1,ax_index);
+        
+        % hold on on plots
+        hold on
+        grid on
+
+        % dpwn sampling instants
+        WindowTime = obj.setup.time(obj.init.temp_time);
+        
+        for traj=1:obj.setup.Ntraj
+            % plot true values
+            y_meas = reshape(obj.init.Y_full_story(traj).val(1,obj.setup.params.dim_out_plot(k),:),size(obj.setup.time));
+            y_true = reshape(obj.init.Ytrue_full_story(traj).val(1,obj.setup.params.dim_out_plot(k),:),size(obj.setup.time));
+            plot(obj.setup.time,y_meas,':','LineWidth',2)
+            set(gca,'ColorOrderIndex',traj)
+            plot(obj.setup.time,y_true,'LineWidth',2)
+            set(gca,'ColorOrderIndex',traj)
+
+            % plot target values    
+            try
+                data = reshape(obj.init.Y_full_story(traj).val(1,obj.setup.params.dim_out_plot(k),obj.init.temp_time),1,length(WindowTime));
+                plot(WindowTime,data,'o','MarkerSize',5);
+            catch 
+                disp('CHECK T_END OR AYELS CONDITION - LOOKS LIKE NO OPTIMISATION HAS BEEN RUN')
+            end
+
+            set(gca,'fontsize', fontsize)
+            ylabel(strcat('y_',num2str(k)));            
+        end
+    end
+    xlabel('simulation time [s]');
+    legend('meas','true','sampled')
+    linkaxes(ax(1:n_subplot-1),'x');
+
+    %%% rover trajectory
+    fig_count = fig_count+1;
+    figure(fig_count)
+    hold on
+    grid on
+    % plot anchors
+    P_a(1,:) = obj.init.params.pos_anchor(1):3:obj.init.params.pos_anchor(end);
+    P_a(2,:) = obj.init.params.pos_anchor(2):3:obj.init.params.pos_anchor(end); 
+    P_a(3,:) = obj.init.params.pos_anchor(3):3:obj.init.params.pos_anchor(end); 
+    for i=1:obj.setup.params.Nanchor
+        plot3(obj.init.X_est(1).val(P_a(1,i),:),obj.init.X_est(1).val(P_a(2,i),:),obj.init.X_est(1).val(P_a(3,i),:),'ko','MarkerSize',10);
+    end
+    % plot rover
+    pos_p = obj.init.params.pos_p;
+    for traj=1:obj.setup.Ntraj 
+        plot3(obj.init.X_est(traj).val(pos_p(1),:),obj.init.X_est(traj).val(pos_p(2),:),obj.init.X_est(traj).val(pos_p(3),:),'--');
+        set(gca,'ColorOrderIndex',traj)
+        plot3(obj.init.X(traj).val(pos_p(1),:),obj.init.X(traj).val(pos_p(2),:),obj.init.X_est(traj).val(pos_p(3),:));    
+    end
+    xlabel('X')
+    ylabel('Y')
+    set(gca,'fontsize', fontsize)
+
+    %%% check distances %%%
+    fig_count = fig_count+1;
+    figure(fig_count)    
+    for n=1:obj.init.params.Nanchor
+        ax(n) = subplot(obj.init.params.Nanchor,1,n);
+        hold on
+        grid on  
+        for traj=1:obj.setup.Ntraj
+            plot(obj.setup.time(obj.init.params.UWB_pos),squeeze(obj.init.Y_full_story(traj).val(1,obj.init.params.pos_dist_out(n),obj.init.params.UWB_pos)),'LineWidth',2);
+            set(gca,'ColorOrderIndex',traj)
+            plot(obj.setup.time(obj.init.params.UWB_pos),squeeze(obj.init.Yhat_full_story(traj).val(1,obj.init.params.pos_dist_out(n),obj.init.params.UWB_pos)),':','LineWidth',2);
+            set(gca,'ColorOrderIndex',traj)
+            plot(obj.setup.time(obj.init.params.UWB_pos),squeeze(obj.init.Ytrue_full_story(traj).val(1,obj.init.params.pos_dist_out(n),obj.init.params.UWB_pos)),'--','LineWidth',2);
+        end        
+        ylabel(['d_',num2str(n)])        
+        set(gca,'fontsize', fontsize)
+    end   
+    xlabel('time [s]')
+    legend('meas','opt','true');            
+    
+end
\ No newline at end of file
diff --git a/Lib/various/plots/testPlot.m b/Lib/various/plots/testPlot.m
new file mode 100644
index 0000000..83e8f55
--- /dev/null
+++ b/Lib/various/plots/testPlot.m
@@ -0,0 +1,9 @@
+%%
+for image=1:4
+    start = [8 12 16 20];
+    figure(image)
+    hold on
+    for i=1:3
+        plot(obs_s{2}.setup.time,obs_s{2}.init.X_est(1).val(start+(i-1),:),'LineWidth',2)
+    end
+end
\ No newline at end of file
diff --git a/Lib/various/routh/license.txt b/Lib/various/routh/license.txt
new file mode 100755
index 0000000..b4685e2
--- /dev/null
+++ b/Lib/various/routh/license.txt
@@ -0,0 +1,24 @@
+Copyright (c) 2016, Edmundo Rivera-Santos
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are
+met:
+
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * 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
+
+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.
diff --git a/Lib/various/routh/myRouth.m b/Lib/various/routh/myRouth.m
new file mode 100755
index 0000000..a6eccd0
--- /dev/null
+++ b/Lib/various/routh/myRouth.m
@@ -0,0 +1,49 @@
+function R = myRouth(b)
+%% ROUTH-HURWITZ Array
+%
+% Examples: 
+%
+% 1. P = s^4 + 10*s^3 + 35*s^2 + 50*s + 24 ;
+%    R = myRouth( [1 10 35 50 24] )
+%
+% 2. syms a b c d s , P = s^4 + a*s^3 + b*s^2 + c*s + d ; 
+%    R = myRouth( [1 a b c d] )
+%
+% 3. syms K , P = s^2 + (12-3*K)*s + 20+0.25*K ; 
+%    R = myRouth( [1 12-3*K 20+0.25*K] )
+
+
+% Ismail Ilker Delice 
+% delice.i@neu.edu
+
+
+%% Polynomial coefficients as input
+if(nargin<1), warning('No Input Argument') ; return
+end
+
+%% Flip vector in left/right direction and find order of polynomial
+b = fliplr(b) ;      
+ord = size(b,2)-1 ; 
+
+% It gives the index number of R11's row  
+% ord = 6,7 --> rou_i = 7
+rou_i = fix( fix(ord/2)*2 ) + 1 ;
+rou_j = ceil((ord+1)/2) ;             % Round infinity
+
+%% If order is even add one zero as a last element of matrix
+Ri = [ b(ord+1:-2:1) ; 
+       b(ord:-2:1)  zeros( fix(( rou_i-1 )/ord) ) ] ;
+
+R = sym( zeros(ord+1,rou_j) )  ;  R(ord+1:-1:ord, : ) = Ri ;
+
+%% All R's for Routh-Hurwitz [Main Algorithm]
+for n = ord-1:-1:2
+for j = 1:round(n/2)
+R(n,j) = ( R(n+1,1)*R(n+2,j+1)-R(n+1,j+1)*R(n+2,1) )/R(n+1) ;
+end
+end
+R(1,1) = R(rou_i,rou_j) ;
+
+%% Simplify and Flip matrix in up/down direction.
+R = simplify(R) ; R = flipud(R) ; 
+
diff --git a/Lib/various/routh/routh.m b/Lib/various/routh/routh.m
new file mode 100755
index 0000000..c4df019
--- /dev/null
+++ b/Lib/various/routh/routh.m
@@ -0,0 +1,81 @@
+function RA=routh(poli,epsilon);
+
+%ROUTH   Routh array.
+%   RA=ROUTH(R,EPSILON) returns the symbolic Routh array RA for 
+%   polynomial R(s). The following special cases are considered:
+%   1) zero first elements and 2) rows of zeros. All zero first 
+%   elements are replaced with the symbolic variable EPSILON
+%   which can be later substituted with positive and negative 
+%   small numbers using SUBS(RA,EPSILON,...). When a row of 
+%   zeros is found, the auxiliary polynomial is used.
+%
+%	Examples:
+%
+%	1) Routh array for s^3+2*s^2+3*s+1
+%
+%		>>syms EPS
+%		>>ra=routh([1 2 3 1],EPS)
+%		ra =
+%
+% 		   1.0000    3.0000
+% 		   2.0000    1.0000
+% 		   2.5000         0
+% 		   1.0000         0
+%
+%	2) Routh array for s^3+a*s^2+b*s+c
+%	
+%		>>syms a b c EPS;
+%		>>ra=routh([1 a b c],EPS);
+%		ra =
+% 
+%		[          1,          b]
+%		[          a,          c]
+%		[ (-c+b*a)/a,          0]
+%		[          c,          0]
+%
+% 
+%   Author:Rivera-Santos, Edmundo J.
+%   E-mail:edmundo@alum.mit.edu
+%
+
+if(nargin<2),
+	fprintf('\nError: Not enough input arguments given.');
+	return
+end
+
+dim=size(poli);		%get size of poli		
+
+coeff=dim(2);				%get number of coefficients
+RA=sym(zeros(coeff,ceil(coeff/2)));	%initialize symbolic Routh array 
+
+for i=1:coeff,
+	RA(2-rem(i,2),ceil(i/2))=poli(i); %assemble 1st and 2nd rows
+end
+
+rows=coeff-2;		%number of rows that need determinants
+index=zeros(rows,1);	%initialize columns-per-row index vector
+
+for i=1:rows,
+	index(rows-i+1)=ceil(i/2); %form index vector from bottom to top
+end
+
+for i=3:coeff,				%go from 3rd row to last
+	if(all(RA(i-1,:)==0)),		%row of zeros
+			fprintf('\nSpecial Case: Row of zeros detected.');
+			a=coeff-i+2;		%order of auxiliary equation
+			b=ceil(a/2)-rem(a,2)+1; %number of auxiliary coefficients
+			temp1=RA(i-2,1:b);	%get auxiliary polynomial
+			temp2=a:-2:0;		%auxiliry polynomial powers
+			RA(i-1,1:b)=temp1.*temp2;	%derivative of auxiliary
+	elseif(RA(i-1,1)==0),		%first element in row is zero
+			fprintf('\nSpecial Case: First element is zero.');
+			RA(i-1,1)=epsilon;	%replace by epsilon
+	end
+				%compute the Routh array elements
+	for j=1:index(i-2),	
+		RA(i,j)=-det([RA(i-2,1) RA(i-2,j+1);RA(i-1,1) RA(i-1,j+1)])/RA(i-1,1);
+	end
+end
+
+	
+	
\ No newline at end of file
diff --git a/Lib/various/sensitivity_battery.m b/Lib/various/sensitivity_battery.m
new file mode 100644
index 0000000..5e31b0b
--- /dev/null
+++ b/Lib/various/sensitivity_battery.m
@@ -0,0 +1,138 @@
+%%
+
+function out = sensitivity_battery(params)
+
+    %% generate data
+    sigma_points = 0;
+    Npoint = 100;
+    x_start = zeros(6,1);
+    x_out = gen_meas(params,x_start,Npoint,sigma_points);
+    SOC = x_out(1,:);
+    Voc = x_out(3,:);
+    R0 = x_out(4,:);
+    R1 = x_out(5,:);
+    C1 = x_out(6,:);
+    
+    %% which window I use?
+    % step = 60;
+    % Istart = randi(length(SOC)-step,1);    
+    % Iend = Istart+step;
+    Istart = 1;
+    Iend = length(SOC);
+    window = Istart:Iend;
+    
+    %% get initial estimate
+    Npoly = 4;    
+    
+    % coefficients
+    % Voc_P = flip(obs.init.X_est_runtime.val(update_vars(1,:),1));
+    % R0_P =  flip(obs.init.X_est_runtime.val(update_vars(2,:),1));
+    % R1_P =  flip(obs.init.X_est_runtime.val(update_vars(3,:),1));
+    % C1_P =  flip(obs.init.X_est_runtime.val(update_vars(4,:),1));
+    Voc_P = polyfit(SOC(window),Voc(window),Npoly);
+    R0_P =  polyfit(SOC(window),R0(window),Npoly);
+    R1_P =  polyfit(SOC(window),R1(window),Npoly);
+    C1_P =  polyfit(SOC(window),C1(window),Npoly);
+    
+    
+    % estimation
+    Voc_est = polyval(Voc_P,SOC(window));
+    R0_est =  polyval(R0_P,SOC(window));
+    R1_est =  polyval(R1_P,SOC(window));
+    C1_est =  polyval(C1_P,SOC(window));
+    
+    %% check effect of parameters
+    Ncheck = 10;
+    Sigma = 1*1e-2;
+    % param_check_id = [1:Npoly];
+    param_check_id = 1;
+    Nparam_check = length(param_check_id);
+    
+    for i=1:Ncheck
+    
+        % init params
+        Voc_P_check(i,:) = Voc_P;
+        R0_P_check(i,:) = R0_P;
+        R1_P_check(i,:) = R1_P;
+        C1_P_check(i,:) = C1_P;
+    
+        % create disturb
+        Voc_P_check(i,param_check_id) = Voc_P(param_check_id).*(1 + Sigma*randn([1 Nparam_check]));
+        R0_P_check(i,param_check_id) = R0_P(param_check_id).*(1 + Sigma*randn([1 Nparam_check]));
+        R1_P_check(i,param_check_id) = R1_P(param_check_id).*(1 + Sigma*randn([1 Nparam_check]));
+        C1_P_check(i,param_check_id) = C1_P(param_check_id).*(1 + Sigma*randn([1 Nparam_check]));
+    
+        % estimate - normal
+        Voc_est_check (i,:) = polyval(Voc_P_check(i,:),SOC(window));
+        R0_est_check(i,:)   =  polyval(R0_P_check(i,:),SOC(window));
+        R1_est_check(i,:)   =  polyval(R1_P_check(i,:),SOC(window));
+        C1_est_check(i,:)   =  polyval(C1_P_check(i,:),SOC(window)); 
+
+        % error
+        out.E_Voc(:,i) = (Voc_est - Voc_est_check(i,:)).^2;
+        out.E_R0(:,i) = (R0_est - R0_est_check(i,:)).^2;
+        out.E_R1(:,i) = (R1_est - R1_est_check(i,:)).^2;
+        out.E_C1(:,i) = (C1_est - C1_est_check(i,:)).^2;
+
+        % RMSE
+        out.E_Voc_RMSE(i) = sqrt(mean(out.E_Voc(:,i)));
+        out.E_R0_RMSE(i) = sqrt(mean(out.E_R0(:,i)));
+        out.E_R1_RMSE(i) = sqrt(mean(out.E_R1(:,i)));
+        out.E_C1_RMSE(i) = sqrt(mean(out.E_C1(:,i)));
+    
+    end
+    
+    %% plots
+    
+    % Voc
+    figure(1)
+    title('Voc')
+    xlabel('SOC')
+    ylabel('Voc')
+    hold on
+    
+    plot(SOC(window),Voc(window),'bo')
+    plot(SOC(window),Voc_est,'r--')
+    for i=1:Ncheck
+        plot(SOC(window),Voc_est_check(i,:),'k:')
+    end
+    
+    % R0
+    figure(2)
+    title('R0')
+    xlabel('SOC')
+    ylabel('R0')
+    hold on
+    
+    plot(SOC(window),R0(window),'bo')
+    plot(SOC(window),R0_est,'r--')
+    for i=1:Ncheck
+        plot(SOC(window),R0_est_check(i,:),'k:')
+    end
+    
+    % R1
+    figure(3)
+    title('R1')
+    xlabel('SOC')
+    ylabel('R1')
+    hold on
+    
+    plot(SOC(window),R1(window),'bo')
+    plot(SOC(window),R1_est,'r--')
+    for i=1:Ncheck
+        plot(SOC(window),R1_est_check(i,:),'k:')
+    end
+    
+    % R1
+    figure(4)
+    title('C1')
+    xlabel('SOC')
+    ylabel('C1')
+    hold on
+    
+    plot(SOC(window),C1(window),'bo')
+    plot(SOC(window),C1_est,'r--')
+    for i=1:Ncheck
+        plot(SOC(window),C1_est_check(i,:),'k:')
+    end
+end
\ No newline at end of file
diff --git a/Lib/various/uwb/J_fcn.m b/Lib/various/uwb/J_fcn.m
new file mode 100644
index 0000000..fa47655
--- /dev/null
+++ b/Lib/various/uwb/J_fcn.m
@@ -0,0 +1,7 @@
+%% cost function with newton 
+function [J, GJ, HJ] = J_fcn(x,y,P_a,D)
+
+    % iterate    
+    tmp = (sqrt( (P_a(1,:)-x).^2 + (P_a(2,:)-y).^2 ) - D);    
+    J = sum(tmp);
+end
\ No newline at end of file
diff --git a/Lib/various/uwb/UWB-bias-last.dat b/Lib/various/uwb/UWB-bias-last.dat
new file mode 100644
index 0000000..94dcc37
--- /dev/null
+++ b/Lib/various/uwb/UWB-bias-last.dat
@@ -0,0 +1,56 @@
+50 23.9651
+60 35.3017
+70 45.695
+80 54.9501
+90 67.8005
+100 80.5486
+110 89.5287
+120 93.3375
+130 100.359
+140 113.833
+150 123.309
+160 134.13
+170 144.688
+180 156.147
+190 169.766
+200 178.771
+210 188.188
+220 196.569
+230 208.532
+240 217.01
+250 228.805
+260 239.863
+270 249.963
+280 258.18
+290 266.733
+300 276.758
+310 291.157
+320 299.9
+330 308.147
+340 321.419
+350 330.91
+360 338.75
+370 347.394
+380 358.97
+390 367.93
+400 379.721
+410 389.135
+420 400.633
+430 410.569
+440 418.752
+450 426.995
+460 439
+470 449.382
+480 457.721
+490 469.591
+500 481.127
+510 488.968
+520 503.868
+530 513.207
+540 519.628
+550 531.763
+560 543.783
+570 552.308
+580 560.152
+590 573.711
+600 585.564
\ No newline at end of file
diff --git a/Lib/various/uwb/biasFitting.m b/Lib/various/uwb/biasFitting.m
new file mode 100644
index 0000000..7bd5aec
--- /dev/null
+++ b/Lib/various/uwb/biasFitting.m
@@ -0,0 +1,41 @@
+function y = biasFitting(value)
+
+tmp  = importdata('UWB-bias-last.dat');
+gt   = tmp(:,1)/100;
+uwb  = tmp(:,2)/100;
+diff = gt - uwb;
+
+[xData, yData] = prepareCurveData( gt, diff );
+ft = fittype( 'smoothingspline' );
+opts = fitoptions( 'Method', 'SmoothingSpline' );
+opts.SmoothingParam = 0.0146215299887771;
+
+% Fit model to data.
+[fitresult, ~] = fit( xData, yData, ft, opts );
+
+% % From the OLD experiments
+% X = [0,0.4,0.5,0.6,0.7,0.8,0.85,1,1.1,2,2.1,2.9,3,4];
+% Y = [0.3,0.3,0.28,0.28,0.25,0.25,0.2,0.2,0.3,0.3,0.25,0.25,0.3,0.3];
+% 
+% [xData, yData] = prepareCurveData( X, Y );
+% 
+% % Set up fittype and options.
+% ft = fittype( 'smoothingspline' );
+% opts = fitoptions( 'Method', 'SmoothingSpline' );
+% opts.Normalize = 'on';
+% opts.SmoothingParam = 0.998414585216365;
+% 
+% % Fit model to data.
+% [fitresult, ~] = fit( xData, yData, ft, opts );
+
+bias = fitresult(value);
+y = bias + value;
+
+% % Plot fit with data.
+% figure( 'Name', 'UWB' );
+% h = plot( fitresult, xData, yData );
+% legend( h, 'Y vs. X', 'UWB', 'Location', 'NorthEast', 'Interpreter', 'none' );
+% % Label axes
+% xlabel( 'X', 'Interpreter', 'none' );
+% ylabel( 'Y', 'Interpreter', 'none' );
+% grid on;
\ No newline at end of file
diff --git a/Lib/various/uwb/get_dist.m b/Lib/various/uwb/get_dist.m
new file mode 100644
index 0000000..6d1db6e
--- /dev/null
+++ b/Lib/various/uwb/get_dist.m
@@ -0,0 +1,12 @@
+%% fcn get distances
+function D = get_dist(p,Pa)
+
+    % init
+    N = size(Pa,2);
+    D = zeros(N,1);
+
+    % compute distances
+    for n = 1:N
+        D(n) = norm(p-Pa(:,n));
+    end
+end
\ No newline at end of file
diff --git a/Lib/various/uwb/test.m b/Lib/various/uwb/test.m
new file mode 100644
index 0000000..a3d496b
--- /dev/null
+++ b/Lib/various/uwb/test.m
@@ -0,0 +1,44 @@
+close all
+clear all
+clc
+
+% anchors
+n = 5;
+% v1 = [0,0,0];
+% v2 = [1,5,0];
+% v3 = [1,-5,5];
+% v4 = [5,0,5];
+% v5 = [15,2,5];
+% 
+% 
+% P_r = [1.5, 1.5, 2];
+% 
+% P_a = zeros(3,n);
+
+v1 = [0,0];
+v2 = [1,5];
+v3 = [1,-5];
+v4 = [5,0];
+v5 = [15,2];
+
+
+P_r = [1.5, 1.5];
+
+P_a = zeros(2,n);
+
+for i=1:n
+    eval(['P_a(:, ' num2str(i) ') = v' num2str(i) ';']); 
+end
+vec = zeros(1,n);
+
+% USAGE: 
+%   P_r must be a vector (1,2) [or (2,1)]
+%   P_a must be a matrix (n,2) [or (2,n)]
+%   bias correction: bool
+%   dist_vec not use at this time -> zeros(1,n)
+%   method: 0 = gradient method, else = newton (suggested)
+%   epsilon: error
+
+uwb_est(P_r, P_a, false, vec, 5, 1e-4, false)
+
+
diff --git a/Lib/various/uwb/uwb_est.m b/Lib/various/uwb/uwb_est.m
new file mode 100644
index 0000000..cbffc58
--- /dev/null
+++ b/Lib/various/uwb/uwb_est.m
@@ -0,0 +1,119 @@
+function Pr_hat = uwb_est(P_r, P_a, bias, dist_vec, method, epsilon, display)
+%UWB_EST Summary of this function goes here
+%   P_r: vector(2,1) - Rover position
+%   P_a: vector(2,n) - Anchor position
+%   bias: bool - bias correction
+%   dist_vec TODO
+%   method: 0 = gradient or 1 = newton, default: Newton
+%   epsilon: float - error
+    
+    % checks
+    % column vector
+    if size(P_r, 1) == 1
+        P_r = P_r';
+    end
+    if size(P_r, 1) ~= 2 
+        disp("ERROR: Rover position size")
+        return
+        
+    end
+    if size(P_a, 1) ~= 2 
+        P_a = P_a';
+    end
+    n = size(P_a,2);
+    if size(P_a, 1) ~= 2 || size(P_a, 2) ~= n
+        disp("ERROR: Anchor position size")
+        return
+    end
+    if display
+        % display method
+        if method == 0
+            disp("Choosen method: Gradient")
+        else
+            disp("Choosen method: Newton")
+        end
+    end
+    % bias correction
+    if bias
+        if size(dist_vec,1) == 1
+            dist_vec = dist_vec';
+        end
+        if size(dist_vec,1) ~= n
+            disp("ERROR: dist_vec size")
+            return
+        end
+        for i=1:n
+            dist_vec(i) = biasFitting(dist_vec(i));
+        end
+    end
+    
+    % calculate exact distance 
+    %d = zeros(1,n)';
+    for i=1:n
+        %d(i) = sqrt((P_r(1)-P_a(1,i))^2+(P_r(2)-P_a(2,i))^2);
+        eval(['d' num2str(i) ' = sqrt((P_a(1,' num2str(i) ')-P_r(1))^2 +(P_a(2,' num2str(i) ')-P_r(2))^2);']);
+    end
+    
+    % amplification factor
+    % TODO
+    % gamma_i depends on dist_vec(i) variance
+    
+    syms x y real
+    
+    J = 0;
+    for i=1:n
+        J = J + eval(['(sqrt((P_a(1,' num2str(i) ')-x)^2 +(P_a(2,' num2str(i) ')-y)^2) -d' num2str(i) ')^2;']);
+    end
+    
+    % one shot calculation of gradient and hessian
+    gradJ =  gradient(J,[x,y]);
+    HessianJ = hessian(J,[x,y]);
+    
+    % iterations depends on the choosen method
+    if method == 0 
+        N = 10;
+    else
+        N = 5;
+    end
+    
+    eta = zeros(2,N);
+    % initial conditions (then will be the previous step approximation)
+    eta(:,1) = [P_r(1),P_r(2)]*1;
+    
+    Jn = zeros(N,1);
+
+    % used with gradient method 
+    gamma = 0.1;
+   
+    tic
+    for k=2:N
+        gradJnum =  eval(subs(gradJ,{x,y},{eta(1,k-1),eta(2,k-1)}));
+        if method == 0
+            eta(:,k) = eta(:,k-1) - gamma*gradJnum;
+        else
+            HessianJnum = eval(subs(HessianJ,{x,y},{eta(1,k-1),eta(2,k-1)}));
+            eta(:,k) = eta(:,k-1) - pinv(HessianJnum)*gradJnum;
+        end
+        
+        Jn(k) = eval(subs(J,{x,y},{eta(1,k-1),eta(2,k-1)}));
+        error = vecnorm(eta(:,k)-P_r,2,1);
+        if error < epsilon
+            break
+        end
+        %fprintf("Iteration:\t%d\n",k);
+    end
+    t = toc;
+    
+    Pr_hat = eta(:,k);
+    
+    if display
+        fprintf("Frequency:\t%d Hz\n\n",1/t);
+        eta
+        error = vecnorm(Pr_hat-P_r)
+        figure(j)
+        plot(Jn)
+        grid on
+        legend('Jn')
+    end
+end
+
diff --git a/Lib/various/uwb/uwb_est_v2.m b/Lib/various/uwb/uwb_est_v2.m
new file mode 100644
index 0000000..a2f3190
--- /dev/null
+++ b/Lib/various/uwb/uwb_est_v2.m
@@ -0,0 +1,107 @@
+function Pr_hat = uwb_est_v2(P_r, P_a, dist_vec, params)
+%UWB_EST Summary of this function goes here
+%   P_r: vector(2,1) - Rover position
+%   P_a: vector(2,n) - Anchor position
+%   bias: bool - bias correction
+%   dist_vec TODO
+%   method: 0 = gradient or 1 = newton, default: Newton
+%   epsilon: float - error
+    
+    % checks
+
+    %%% column vector    
+    P_r = reshape(P_r,length(P_r),1);    
+    if size(P_r, 1) ~= 2 
+        error('ERROR: Rover position size');
+        return
+    end
+
+    %%% anchor points
+    if size(P_a, 1) ~= 2 
+        P_a = P_a';
+    end
+    n = size(P_a,2);
+    if size(P_a, 2) ~= params.Nanchor
+        error('ERROR: Anchor position size')
+        return
+    end
+
+    %%% display
+    if params.display_uwb
+        % display method
+        if method == 0
+            disp("Choosen method: Gradient")
+        else
+            disp("Choosen method: Newton")
+        end
+    end
+
+    %%% bias correction
+    if params.bias
+        dist_vec = reshape(dist_vec,length(dist_vec),1);        
+        if size(dist_vec,1) ~= params.Nanchor
+            error('ERROR: dist_vec size')
+            return
+        end
+        for i=1:n
+            dist_vec(i) = biasFitting(dist_vec(i));
+        end
+    end
+    
+    % calculate exact distance 
+    %D_vec(1,:) = sqrt( (P_a(1,:)-P_r(1)).^2 + (P_a(2,:)-P_r(2)).^2);
+
+    % get distance from meas
+    D_vec(1,:) = dist_vec;
+    
+    % amplification factor
+    % TODO
+    % gamma_i depends on dist_vec(i) variance
+    
+    N = params.grad_Niter+1;
+    
+    eta = zeros(2,N);
+    % initial conditions (then will be the previous step approximation)
+    eta(:,1) = [P_r(1),P_r(2)]*1;
+    
+    Jn = zeros(N,1);
+
+    % used with gradient method 
+    gamma = 0.1;
+   
+    % test: No optimization
+    %N = 1;    
+
+    tic
+    for k=2:N
+        gradJnum =  params.GJ(eta(1,k-1),eta(2,k-1),P_a,D_vec,params);
+        if params.method == 0
+            eta(:,k) = eta(:,k-1) - gamma*gradJnum;
+        else
+            HessianJnum = params.HJ(eta(1,k-1),eta(2,k-1),P_a,D_vec,params);
+            eta(:,k) = eta(:,k-1) - pinv(HessianJnum)*gradJnum;
+        end
+        
+        Jn(k) = params.J(eta(1,k-1),eta(2,k-1),P_a,D_vec);
+        error = vecnorm(eta(:,k)-P_r,2,1);
+        if error < params.epsilon
+            break
+        end
+        %fprintf("Iteration:\t%d\n",k);
+    end
+    t = toc;
+    
+    %k = 1;
+    Pr_hat = eta(:,k);
+    
+    if params.display_uwb
+        fprintf("Frequency:\t%d Hz\n\n",1/t);
+        eta
+        error = vecnorm(Pr_hat-P_r)
+        figure(j)
+        plot(Jn)
+        grid on
+        legend('Jn')
+    end
+end
+
diff --git a/Lib/various/wavelets/battery_meas.mat b/Lib/various/wavelets/battery_meas.mat
new file mode 100644
index 0000000..7e084b9
Binary files /dev/null and b/Lib/various/wavelets/battery_meas.mat differ
diff --git a/Lib/various/wavelets/example.m b/Lib/various/wavelets/example.m
new file mode 100644
index 0000000..59356f1
--- /dev/null
+++ b/Lib/various/wavelets/example.m
@@ -0,0 +1,81 @@
+%% matlab example on wavelet analysis
+
+% load dataset
+clear; close all; clc;
+
+if 0
+%%% battery dataset
+load Lib/various/wavelets/battery_meas.mat; 
+else
+%%% fancy dataset
+% sine wave: y_true = 5*sin(0.5*time) + 10*sin(0.1*time); (1% noise gaussian)
+load Lib/various/wavelets/sinewave_meas.mat; 
+end
+
+% reframe data
+time_long = time;
+y_meas_long = y_meas;
+y_true_long = y_true;
+start = 1;
+stop = 600;
+time = time_long(start:stop);
+y_meas = y_meas_long(start:stop);
+% define sampling frequency
+Fs = 1; % 1 Hz
+Ts = 1/Fs;
+wvname = 'amor';
+
+%% plot signal
+% plot
+figure;
+plot(time,y_meas);
+grid on
+xlabel('t [s]');
+ylabel('V [V]');
+
+%% time frequency using STFT
+figure;
+spectrogram(y_meas,[],[],[],Fs,'yaxis');
+title('Spectrogram')
+
+%% time frequency using STFT
+figure;
+% define sampling frequency
+spectrogram(y_meas,64,[],[],Fs,'yaxis');
+title('Spectrogram: shorter window')
+
+%% time frequency using wavelets
+figure;
+cwt(y_meas,wvname,Fs);
+title('CWT analysis')
+
+%% richer frequency analysis with wavelets
+figure;
+Nv = 48;
+
+if 0
+% period constraint
+t = seconds(Ts);
+PLIMITS = [5e0*t 12e1*t];
+cwt(y_meas,wvname,t,'VoicesPerOctave',Nv,'PeriodLimits',PLIMITS);
+else
+% frequency constraint
+PLIMITS = [5e0*Ts 12e1*Ts];
+FLIMITS = fliplr(1./PLIMITS);
+cwt(y_meas,wvname,Fs,'VoicesPerOctave',Nv,'FrequencyLimits',FLIMITS);
+end
+
+
+title('CWT analysis: richer')
+
+%% reconstruct the signal
+[WT, F] = cwt(y_meas,Fs);
+yrec = icwt(WT, wvname, F, [F(end) F(1)], 'SignalMean', mean(y_meas));
+figure;
+grid on
+subplot(2,1,1)
+plot(time,y_meas,time,yrec);
+subplot(2,1,2)
+heatmap(time,F,real(WT))
+grid off
+colormap jet
\ No newline at end of file
diff --git a/Lib/various/wavelets/sinewave_meas.mat b/Lib/various/wavelets/sinewave_meas.mat
new file mode 100644
index 0000000..9396b32
Binary files /dev/null and b/Lib/various/wavelets/sinewave_meas.mat differ
diff --git a/data/.DS_Store b/data/.DS_Store
new file mode 100644
index 0000000..32cfa82
Binary files /dev/null and b/data/.DS_Store differ
diff --git a/data/CALCE/.DS_Store b/data/CALCE/.DS_Store
new file mode 100644
index 0000000..7f050c3
Binary files /dev/null and b/data/CALCE/.DS_Store differ
diff --git a/data/CALCE/INR_18650/.DS_Store b/data/CALCE/INR_18650/.DS_Store
new file mode 100644
index 0000000..79414d0
Binary files /dev/null and b/data/CALCE/INR_18650/.DS_Store differ
diff --git a/data/CALCE/INR_18650/BJDST/SP2_25C_BJDST/11_12_2015_SP20-2_BJDST_80SOC.xlsx b/data/CALCE/INR_18650/BJDST/SP2_25C_BJDST/11_12_2015_SP20-2_BJDST_80SOC.xlsx
new file mode 100644
index 0000000..84bf6c8
Binary files /dev/null and b/data/CALCE/INR_18650/BJDST/SP2_25C_BJDST/11_12_2015_SP20-2_BJDST_80SOC.xlsx differ
diff --git a/data/CALCE/INR_18650/BJDST/SP2_25C_BJDST/11_13_2015_SP20-2_BJDST_50SOC.xlsx b/data/CALCE/INR_18650/BJDST/SP2_25C_BJDST/11_13_2015_SP20-2_BJDST_50SOC.xlsx
new file mode 100644
index 0000000..f9c71b0
Binary files /dev/null and b/data/CALCE/INR_18650/BJDST/SP2_25C_BJDST/11_13_2015_SP20-2_BJDST_50SOC.xlsx differ
diff --git a/data/CALCE/INR_18650/DST/.DS_Store b/data/CALCE/INR_18650/DST/.DS_Store
new file mode 100644
index 0000000..37e0520
Binary files /dev/null and b/data/CALCE/INR_18650/DST/.DS_Store differ
diff --git a/data/CALCE/INR_18650/DST/10_16_2015_Initial capacity_SP20-2.xls b/data/CALCE/INR_18650/DST/10_16_2015_Initial capacity_SP20-2.xls
new file mode 100644
index 0000000..bc21a64
Binary files /dev/null and b/data/CALCE/INR_18650/DST/10_16_2015_Initial capacity_SP20-2.xls differ
diff --git a/data/CALCE/INR_18650/DST/SP2_0C_DST/02_24_2016_SP20-2_0C_DST_50SOC.xlsx b/data/CALCE/INR_18650/DST/SP2_0C_DST/02_24_2016_SP20-2_0C_DST_50SOC.xlsx
new file mode 100644
index 0000000..4f511c7
Binary files /dev/null and b/data/CALCE/INR_18650/DST/SP2_0C_DST/02_24_2016_SP20-2_0C_DST_50SOC.xlsx differ
diff --git a/data/CALCE/INR_18650/DST/SP2_0C_DST/02_24_2016_SP20-2_0C_DST_80SOC.xls b/data/CALCE/INR_18650/DST/SP2_0C_DST/02_24_2016_SP20-2_0C_DST_80SOC.xls
new file mode 100644
index 0000000..5546cb8
Binary files /dev/null and b/data/CALCE/INR_18650/DST/SP2_0C_DST/02_24_2016_SP20-2_0C_DST_80SOC.xls differ
diff --git a/data/CALCE/INR_18650/DST/SP2_25C_DST/11_05_2015_SP20-2_DST_50SOC.xlsx b/data/CALCE/INR_18650/DST/SP2_25C_DST/11_05_2015_SP20-2_DST_50SOC.xlsx
new file mode 100644
index 0000000..045ad8b
Binary files /dev/null and b/data/CALCE/INR_18650/DST/SP2_25C_DST/11_05_2015_SP20-2_DST_50SOC.xlsx differ
diff --git a/data/CALCE/INR_18650/DST/SP2_25C_DST/11_05_2015_SP20-2_DST_80SOC.xlsx b/data/CALCE/INR_18650/DST/SP2_25C_DST/11_05_2015_SP20-2_DST_80SOC.xlsx
new file mode 100644
index 0000000..022eff2
Binary files /dev/null and b/data/CALCE/INR_18650/DST/SP2_25C_DST/11_05_2015_SP20-2_DST_80SOC.xlsx differ
diff --git a/data/CALCE/INR_18650/DST/SP2_45C_DST/12_11_2015_SP20-2_45C_DST_50SOC.xlsx b/data/CALCE/INR_18650/DST/SP2_45C_DST/12_11_2015_SP20-2_45C_DST_50SOC.xlsx
new file mode 100644
index 0000000..078e6d6
Binary files /dev/null and b/data/CALCE/INR_18650/DST/SP2_45C_DST/12_11_2015_SP20-2_45C_DST_50SOC.xlsx differ
diff --git a/data/CALCE/INR_18650/DST/SP2_45C_DST/12_11_2015_SP20-2_45C_DST_80SOC.xls b/data/CALCE/INR_18650/DST/SP2_45C_DST/12_11_2015_SP20-2_45C_DST_80SOC.xls
new file mode 100644
index 0000000..307bcaa
Binary files /dev/null and b/data/CALCE/INR_18650/DST/SP2_45C_DST/12_11_2015_SP20-2_45C_DST_80SOC.xls differ
diff --git a/data/CALCE/INR_18650/FUDS/SP2_25C_FUDS/11_06_2015_SP20-2_FUDS_80SOC.xlsx b/data/CALCE/INR_18650/FUDS/SP2_25C_FUDS/11_06_2015_SP20-2_FUDS_80SOC.xlsx
new file mode 100644
index 0000000..74f2bd0
Binary files /dev/null and b/data/CALCE/INR_18650/FUDS/SP2_25C_FUDS/11_06_2015_SP20-2_FUDS_80SOC.xlsx differ
diff --git a/data/CALCE/INR_18650/FUDS/SP2_25C_FUDS/11_09_2015_SP20-2_FUDS_50SOC.xlsx b/data/CALCE/INR_18650/FUDS/SP2_25C_FUDS/11_09_2015_SP20-2_FUDS_50SOC.xlsx
new file mode 100644
index 0000000..dce4988
Binary files /dev/null and b/data/CALCE/INR_18650/FUDS/SP2_25C_FUDS/11_09_2015_SP20-2_FUDS_50SOC.xlsx differ
diff --git a/data/CALCE/INR_18650/SOC_OCV/25C_incremental/SOC_OCV.m b/data/CALCE/INR_18650/SOC_OCV/25C_incremental/SOC_OCV.m
new file mode 100644
index 0000000..cea124f
--- /dev/null
+++ b/data/CALCE/INR_18650/SOC_OCV/25C_incremental/SOC_OCV.m
@@ -0,0 +1,45 @@
+clear all;
+
+% Load the Excel file
+file_path = "/Users/tkddesai/surfdrive/Main drive/Projects/MatLab/obsopt_pkg/data/CALCE/INR_18650/SOC_OCV/25C_incremental/12_2_2015_Incremental OCV test_SP20-3.xlsx";
+
+% Get sheet names
+[~, sheet_names] = xlsfinfo(file_path);
+
+% Exclude the 'Info' sheet
+sheet_names(strcmp(sheet_names, 'Info')) = [];
+
+% Initialize variables to store rows before step index changes to 5
+rows_before_step_change_all_sheets = {};
+
+% Loop through each sheet
+for i = 1:length(sheet_names)
+    % Read the data from the current sheet
+    data = readtable(file_path, 'Sheet', sheet_names{i});
+    
+    % Extract relevant columns
+    step_index = data.Step_Index;
+    
+    % Find the indices where the step index changes to 5 and is not already 5
+    step_change_indices = find((step_index == 5 |step_index == 9) & ([0; diff(step_index)] ~= 0));
+    
+    % Get the row just before each step index change to 5
+    rows_before_step_change = step_change_indices - 1;
+    
+    % Handle cases where the index might be out of bounds
+    rows_before_step_change(rows_before_step_change < 1) = [];
+    
+    % Save the rows before step index changes to 5
+    rows_before_step_change_all_sheets{i} = data(rows_before_step_change, :);
+end
+
+% Combine all rows before step index changes to 5 into one table
+all_rows_before_step_change = vertcat(rows_before_step_change_all_sheets{:});
+
+% Display the results
+disp('Rows before step index changes to 5:');
+disp(all_rows_before_step_change);
+
+% Save to a new Excel file
+output_file_path = 'rows_before_step_changes_3.xlsx';
+writetable(all_rows_before_step_change, output_file_path);
\ No newline at end of file
diff --git a/data/CALCE/INR_18650/Sample_1_SOC_incremental/.DS_Store b/data/CALCE/INR_18650/Sample_1_SOC_incremental/.DS_Store
new file mode 100644
index 0000000..5008ddf
Binary files /dev/null and b/data/CALCE/INR_18650/Sample_1_SOC_incremental/.DS_Store differ
diff --git a/data/CALCE/INR_18650/Sample_1_SOC_incremental/SP1_0C_IC_OCV_02_26_2016.xlsx b/data/CALCE/INR_18650/Sample_1_SOC_incremental/SP1_0C_IC_OCV_02_26_2016.xlsx
new file mode 100644
index 0000000..39e890a
Binary files /dev/null and b/data/CALCE/INR_18650/Sample_1_SOC_incremental/SP1_0C_IC_OCV_02_26_2016.xlsx differ
diff --git a/data/CALCE/INR_18650/Sample_1_SOC_incremental/SP1_25C_IC_OCV_12_2_2015.xlsx b/data/CALCE/INR_18650/Sample_1_SOC_incremental/SP1_25C_IC_OCV_12_2_2015.xlsx
new file mode 100644
index 0000000..14a1506
Binary files /dev/null and b/data/CALCE/INR_18650/Sample_1_SOC_incremental/SP1_25C_IC_OCV_12_2_2015.xlsx differ
diff --git a/data/CALCE/INR_18650/Sample_1_SOC_incremental/SP1_45C_IC_OCV_12_09_2015.xlsx b/data/CALCE/INR_18650/Sample_1_SOC_incremental/SP1_45C_IC_OCV_12_09_2015.xlsx
new file mode 100644
index 0000000..fef06c1
Binary files /dev/null and b/data/CALCE/INR_18650/Sample_1_SOC_incremental/SP1_45C_IC_OCV_12_09_2015.xlsx differ
diff --git a/data/CALCE/INR_18650/Sample_1_SOC_incremental/SP1_Initial capacity_10_16_2015.xlsx b/data/CALCE/INR_18650/Sample_1_SOC_incremental/SP1_Initial capacity_10_16_2015.xlsx
new file mode 100644
index 0000000..3b28715
Binary files /dev/null and b/data/CALCE/INR_18650/Sample_1_SOC_incremental/SP1_Initial capacity_10_16_2015.xlsx differ
diff --git a/data/CALCE/INR_18650/Sample_1_SOC_incremental/matlab.mat b/data/CALCE/INR_18650/Sample_1_SOC_incremental/matlab.mat
new file mode 100644
index 0000000..ee56d05
Binary files /dev/null and b/data/CALCE/INR_18650/Sample_1_SOC_incremental/matlab.mat differ
diff --git a/data/CALCE/INR_18650/Sample_1_SOC_incremental/~$SP1_45C_IC_OCV_12_09_2015.xlsx b/data/CALCE/INR_18650/Sample_1_SOC_incremental/~$SP1_45C_IC_OCV_12_09_2015.xlsx
new file mode 100644
index 0000000..29b2fba
Binary files /dev/null and b/data/CALCE/INR_18650/Sample_1_SOC_incremental/~$SP1_45C_IC_OCV_12_09_2015.xlsx differ
diff --git a/data/CALCE/INR_18650/US06/SP2_25C_US06/11_11_2015_SP20-2_US06_50SOC.xlsx b/data/CALCE/INR_18650/US06/SP2_25C_US06/11_11_2015_SP20-2_US06_50SOC.xlsx
new file mode 100644
index 0000000..0d045a2
Binary files /dev/null and b/data/CALCE/INR_18650/US06/SP2_25C_US06/11_11_2015_SP20-2_US06_50SOC.xlsx differ
diff --git a/data/CALCE/INR_18650/US06/SP2_25C_US06/11_11_2015_SP20-2_US06_80SOC.xlsx b/data/CALCE/INR_18650/US06/SP2_25C_US06/11_11_2015_SP20-2_US06_80SOC.xlsx
new file mode 100644
index 0000000..6a7a351
Binary files /dev/null and b/data/CALCE/INR_18650/US06/SP2_25C_US06/11_11_2015_SP20-2_US06_80SOC.xlsx differ
diff --git a/data/CALCE/INR_18650/~$SP1_45C_IC_OCV_12_09_2015.xlsx b/data/CALCE/INR_18650/~$SP1_45C_IC_OCV_12_09_2015.xlsx
new file mode 100644
index 0000000..29b2fba
Binary files /dev/null and b/data/CALCE/INR_18650/~$SP1_45C_IC_OCV_12_09_2015.xlsx differ
diff --git a/data/DST_Current_input_SP2_25C_DST_50.xlsx b/data/DST_Current_input_SP2_25C_DST_50.xlsx
new file mode 100644
index 0000000..9da4ed0
Binary files /dev/null and b/data/DST_Current_input_SP2_25C_DST_50.xlsx differ
diff --git a/data/ECM_parameters.mat b/data/ECM_parameters.mat
new file mode 100644
index 0000000..7891c21
Binary files /dev/null and b/data/ECM_parameters.mat differ
diff --git a/data/ECM_parameters_fedeoli.mat b/data/ECM_parameters_fedeoli.mat
new file mode 100644
index 0000000..c462e12
Binary files /dev/null and b/data/ECM_parameters_fedeoli.mat differ
diff --git a/data/ECM_parameters_updated.mat b/data/ECM_parameters_updated.mat
new file mode 100644
index 0000000..ff4abfc
Binary files /dev/null and b/data/ECM_parameters_updated.mat differ
diff --git a/data/Intial Para data.txt b/data/Intial Para data.txt
new file mode 100644
index 0000000..932a78c
--- /dev/null
+++ b/data/Intial Para data.txt	
@@ -0,0 +1,13 @@
+    % system parameters
+    % battery EMF
+    params.Voc = 3.5;
+    % ohmic internal resistance
+    params.R0 = .0125;
+    % polarization resistance
+    params.R1 = .040;
+    % polarization capacity
+    params.C1 = 500;
+    % Battery Capacity (converting Ampere-hour to Ampere-second)
+    params.C_n = 4.1 * 3600; 
+    % Battery charging-discharging efficiency (for Li-ion=100%)
+    params.eta = 1;
\ No newline at end of file
diff --git a/data/SP2_25C_DST/11_05_2015_SP20-2_DST_50SOC.xls b/data/SP2_25C_DST/11_05_2015_SP20-2_DST_50SOC.xls
new file mode 100644
index 0000000..d6e6680
Binary files /dev/null and b/data/SP2_25C_DST/11_05_2015_SP20-2_DST_50SOC.xls differ
diff --git a/data/SP2_25C_DST/11_05_2015_SP20-2_DST_80SOC.xls b/data/SP2_25C_DST/11_05_2015_SP20-2_DST_80SOC.xls
new file mode 100644
index 0000000..022eff2
Binary files /dev/null and b/data/SP2_25C_DST/11_05_2015_SP20-2_DST_80SOC.xls differ
diff --git a/data/SP2_25C_DST/SP2_25C_DST_50.xlsx b/data/SP2_25C_DST/SP2_25C_DST_50.xlsx
new file mode 100644
index 0000000..5e14fb9
Binary files /dev/null and b/data/SP2_25C_DST/SP2_25C_DST_50.xlsx differ
diff --git a/obs_default.mat b/obs_default.mat
new file mode 100644
index 0000000..3e94217
Binary files /dev/null and b/obs_default.mat differ
diff --git a/parameters.json b/parameters.json
new file mode 100644
index 0000000..8be44a5
--- /dev/null
+++ b/parameters.json
@@ -0,0 +1,54 @@
+{
+    "subAxes": 
+        {
+            "Color": "none",
+            "LineWidth": 1.2,
+            "XGrid": "off",
+            "YGrid": "off",
+            "ZGrid": "off",
+            "GridAlpha": 0.15,
+            "GridColor": [0.15, 0.15, 0.15],
+            "GridLineStyle": "-",
+            "Box": "on",
+            "TickDir": "in",
+            "Comments": "theme of the sub axes"
+        },
+
+    "zoomedArea": 
+        {
+            "Color": "k",
+            "FaceColor": "none",
+            "FaceAlpha": 0,
+            "LineStyle": "-",
+            "LineWidth": 1.5,
+            "Comments": "theme of the zoomed area"
+        },
+
+     "dynamicRect": 
+        {
+            "LineColor": [0, 0.4471, 0.7412],
+            "LineWidth": 2,
+            "MarkerSize": 9,
+            "FaceColor": [0, 0.4471, 0.7412],
+            "FaceAspect": 0.3,
+            "EdgeColor": "k",
+            "Comments": "theme of the dynamic rectangle"
+        },
+
+     "connection": 
+        {
+            "LineNumber": 2, 
+            "LineColor": "k",
+            "LineWidth": 1.5,
+            "LineStyle": ":",
+            "StartHeadStyle": "ellipse",
+            "StartHeadLength": 3,
+            "StartHeadWidth": 3,
+            "EndHeadStyle": "cback2",
+            "EndHeadLength": 7,
+            "EndHeadWidth": 7,
+            "Comments": "theme of the connected lines"
+        }
+
+}
+
diff --git a/results/simulations/battery/JournalTests/CALCE_4POLY_PARALLEL_ADAPTIVE_WAVELETS_NFAST30_NTSFAST01_NSLOW30_NTSSLOW30_Z0_09_2ITER.mat b/results/simulations/battery/JournalTests/CALCE_4POLY_PARALLEL_ADAPTIVE_WAVELETS_NFAST30_NTSFAST01_NSLOW30_NTSSLOW30_Z0_09_2ITER.mat
new file mode 100644
index 0000000..23e51f9
Binary files /dev/null and b/results/simulations/battery/JournalTests/CALCE_4POLY_PARALLEL_ADAPTIVE_WAVELETS_NFAST30_NTSFAST01_NSLOW30_NTSSLOW30_Z0_09_2ITER.mat differ
diff --git a/results/simulations/battery/JournalTests/CALCE_4POLY_PARALLEL_FIXEDSAMPLE_NFAST30_NTSFAST01_NSLOW30_NTSSLOW100_Z0_09_2ITER.mat b/results/simulations/battery/JournalTests/CALCE_4POLY_PARALLEL_FIXEDSAMPLE_NFAST30_NTSFAST01_NSLOW30_NTSSLOW100_Z0_09_2ITER.mat
new file mode 100644
index 0000000..cca8092
Binary files /dev/null and b/results/simulations/battery/JournalTests/CALCE_4POLY_PARALLEL_FIXEDSAMPLE_NFAST30_NTSFAST01_NSLOW30_NTSSLOW100_Z0_09_2ITER.mat differ
diff --git a/results/simulations/battery/JournalTests/CALCE_4POLY_SINGLE_ADAPTIVE_WAVELETS_N30_NTS30_Z0_09_2ITER._FNTS20.mat b/results/simulations/battery/JournalTests/CALCE_4POLY_SINGLE_ADAPTIVE_WAVELETS_N30_NTS30_Z0_09_2ITER._FNTS20.mat
new file mode 100644
index 0000000..e9f0684
Binary files /dev/null and b/results/simulations/battery/JournalTests/CALCE_4POLY_SINGLE_ADAPTIVE_WAVELETS_N30_NTS30_Z0_09_2ITER._FNTS20.mat differ
diff --git a/results/simulations/battery/JournalTests/CALCE_4POLY_SINGLE_ADAPTIVE_WAVELETS_N30_NTS30_Z0_09_2ITER_FNTS05.mat b/results/simulations/battery/JournalTests/CALCE_4POLY_SINGLE_ADAPTIVE_WAVELETS_N30_NTS30_Z0_09_2ITER_FNTS05.mat
new file mode 100644
index 0000000..6d4001d
Binary files /dev/null and b/results/simulations/battery/JournalTests/CALCE_4POLY_SINGLE_ADAPTIVE_WAVELETS_N30_NTS30_Z0_09_2ITER_FNTS05.mat differ
diff --git a/results/simulations/battery/JournalTests/CALCE_4POLY_SINGLE_ADAPTIVE_WAVELETS_N30_NTS30_Z0_09_2ITER_FNTS15.mat b/results/simulations/battery/JournalTests/CALCE_4POLY_SINGLE_ADAPTIVE_WAVELETS_N30_NTS30_Z0_09_2ITER_FNTS15.mat
new file mode 100644
index 0000000..ce32736
Binary files /dev/null and b/results/simulations/battery/JournalTests/CALCE_4POLY_SINGLE_ADAPTIVE_WAVELETS_N30_NTS30_Z0_09_2ITER_FNTS15.mat differ
diff --git a/results/simulations/battery/JournalTests/CALCE_4POLY_SINGLE_FIXEDSAMPLE_N30_NTS30_Z0_09_2ITER.mat b/results/simulations/battery/JournalTests/CALCE_4POLY_SINGLE_FIXEDSAMPLE_N30_NTS30_Z0_09_2ITER.mat
new file mode 100644
index 0000000..3e1ef5f
Binary files /dev/null and b/results/simulations/battery/JournalTests/CALCE_4POLY_SINGLE_FIXEDSAMPLE_N30_NTS30_Z0_09_2ITER.mat differ
diff --git a/results/simulations/battery/JournalTests/INR_18650/CALCE_INR_18650__DST_4POLY_PARALLEL_ADAPTIVA_WAVELETS_NFAST30_NTSFASTMIN1_FFASTMIN1_NSLOW30_NTSSLOW100_NTSSLOWMIN50_FSLOWMIN001_Z0_09_2ITER.mat b/results/simulations/battery/JournalTests/INR_18650/CALCE_INR_18650__DST_4POLY_PARALLEL_ADAPTIVA_WAVELETS_NFAST30_NTSFASTMIN1_FFASTMIN1_NSLOW30_NTSSLOW100_NTSSLOWMIN50_FSLOWMIN001_Z0_09_2ITER.mat
new file mode 100644
index 0000000..e92817e
Binary files /dev/null and b/results/simulations/battery/JournalTests/INR_18650/CALCE_INR_18650__DST_4POLY_PARALLEL_ADAPTIVA_WAVELETS_NFAST30_NTSFASTMIN1_FFASTMIN1_NSLOW30_NTSSLOW100_NTSSLOWMIN50_FSLOWMIN001_Z0_09_2ITER.mat differ
diff --git a/results/simulations/battery/JournalTests/INR_18650/CALCE_INR_18650__DST_4POLY_PARALLEL_FIXEDSAMPLE_NFAST30_NTSFAST1_NSLOW30_NTSSLOW100_Z0_09_2ITER.mat b/results/simulations/battery/JournalTests/INR_18650/CALCE_INR_18650__DST_4POLY_PARALLEL_FIXEDSAMPLE_NFAST30_NTSFAST1_NSLOW30_NTSSLOW100_Z0_09_2ITER.mat
new file mode 100644
index 0000000..e4037bd
Binary files /dev/null and b/results/simulations/battery/JournalTests/INR_18650/CALCE_INR_18650__DST_4POLY_PARALLEL_FIXEDSAMPLE_NFAST30_NTSFAST1_NSLOW30_NTSSLOW100_Z0_09_2ITER.mat differ
diff --git a/results/simulations/battery/JournalTests/INR_18650/CALCE_INR_18650__DST_4POLY_SINGLE_ADAPTIVE_WAVELETS_N30_NTS30_Z0_09_2ITER_FNTS05_FMIN05.mat b/results/simulations/battery/JournalTests/INR_18650/CALCE_INR_18650__DST_4POLY_SINGLE_ADAPTIVE_WAVELETS_N30_NTS30_Z0_09_2ITER_FNTS05_FMIN05.mat
new file mode 100644
index 0000000..fb5553b
Binary files /dev/null and b/results/simulations/battery/JournalTests/INR_18650/CALCE_INR_18650__DST_4POLY_SINGLE_ADAPTIVE_WAVELETS_N30_NTS30_Z0_09_2ITER_FNTS05_FMIN05.mat differ
diff --git a/results/simulations/battery/JournalTests/INR_18650/CALCE_INR_18650__DST_4POLY_SINGLE_ADAPTIVE_WAVELETS_N30_NTS30_Z0_09_2ITER_FNTS05_FMIN1.mat b/results/simulations/battery/JournalTests/INR_18650/CALCE_INR_18650__DST_4POLY_SINGLE_ADAPTIVE_WAVELETS_N30_NTS30_Z0_09_2ITER_FNTS05_FMIN1.mat
new file mode 100644
index 0000000..9118c62
Binary files /dev/null and b/results/simulations/battery/JournalTests/INR_18650/CALCE_INR_18650__DST_4POLY_SINGLE_ADAPTIVE_WAVELETS_N30_NTS30_Z0_09_2ITER_FNTS05_FMIN1.mat differ
diff --git a/results/simulations/battery/JournalTests/INR_18650/CALCE_INR_18650__DST_4POLY_SINGLE_ADAPTIVE_WAVELETS_N30_NTS30_Z0_09_2ITER_FNTS10_FMIN05.mat b/results/simulations/battery/JournalTests/INR_18650/CALCE_INR_18650__DST_4POLY_SINGLE_ADAPTIVE_WAVELETS_N30_NTS30_Z0_09_2ITER_FNTS10_FMIN05.mat
new file mode 100644
index 0000000..5400095
Binary files /dev/null and b/results/simulations/battery/JournalTests/INR_18650/CALCE_INR_18650__DST_4POLY_SINGLE_ADAPTIVE_WAVELETS_N30_NTS30_Z0_09_2ITER_FNTS10_FMIN05.mat differ
diff --git a/results/simulations/battery/JournalTests/INR_18650/CALCE_INR_18650__DST_4POLY_SINGLE_ADAPTIVE_WAVELETS_N30_NTS30_Z0_09_2ITER_FNTS10_FMIN1.mat b/results/simulations/battery/JournalTests/INR_18650/CALCE_INR_18650__DST_4POLY_SINGLE_ADAPTIVE_WAVELETS_N30_NTS30_Z0_09_2ITER_FNTS10_FMIN1.mat
new file mode 100644
index 0000000..33d792c
Binary files /dev/null and b/results/simulations/battery/JournalTests/INR_18650/CALCE_INR_18650__DST_4POLY_SINGLE_ADAPTIVE_WAVELETS_N30_NTS30_Z0_09_2ITER_FNTS10_FMIN1.mat differ
diff --git a/results/simulations/battery/JournalTests/INR_18650/CALCE_INR_18650__DST_4POLY_SINGLE_ADAPTIVE_WAVELETS_N30_NTS30_Z0_09_2ITER_FNTS20_FMIN05.mat b/results/simulations/battery/JournalTests/INR_18650/CALCE_INR_18650__DST_4POLY_SINGLE_ADAPTIVE_WAVELETS_N30_NTS30_Z0_09_2ITER_FNTS20_FMIN05.mat
new file mode 100644
index 0000000..1856d29
Binary files /dev/null and b/results/simulations/battery/JournalTests/INR_18650/CALCE_INR_18650__DST_4POLY_SINGLE_ADAPTIVE_WAVELETS_N30_NTS30_Z0_09_2ITER_FNTS20_FMIN05.mat differ
diff --git a/results/simulations/battery/JournalTests/INR_18650/CALCE_INR_18650__DST_4POLY_SINGLE_ADAPTIVE_WAVELETS_N30_NTS30_Z0_09_2ITER_FNTS20_FMIN1.mat b/results/simulations/battery/JournalTests/INR_18650/CALCE_INR_18650__DST_4POLY_SINGLE_ADAPTIVE_WAVELETS_N30_NTS30_Z0_09_2ITER_FNTS20_FMIN1.mat
new file mode 100644
index 0000000..bdd4f28
Binary files /dev/null and b/results/simulations/battery/JournalTests/INR_18650/CALCE_INR_18650__DST_4POLY_SINGLE_ADAPTIVE_WAVELETS_N30_NTS30_Z0_09_2ITER_FNTS20_FMIN1.mat differ
diff --git a/results/simulations/battery/JournalTests/INR_18650/CALCE_INR_18650__DST_4POLY_SINGLE_FIXEDSAMPLE_N30_NTS30_Z0_09_2ITER.mat b/results/simulations/battery/JournalTests/INR_18650/CALCE_INR_18650__DST_4POLY_SINGLE_FIXEDSAMPLE_N30_NTS30_Z0_09_2ITER.mat
new file mode 100644
index 0000000..3048890
Binary files /dev/null and b/results/simulations/battery/JournalTests/INR_18650/CALCE_INR_18650__DST_4POLY_SINGLE_FIXEDSAMPLE_N30_NTS30_Z0_09_2ITER.mat differ
diff --git a/results/simulations/battery/report/6500s_1005_noise1E-2_adaptiveNO_filterNO_filteredMeas.mat b/results/simulations/battery/report/6500s_1005_noise1E-2_adaptiveNO_filterNO_filteredMeas.mat
new file mode 100644
index 0000000..5d9f217
Binary files /dev/null and b/results/simulations/battery/report/6500s_1005_noise1E-2_adaptiveNO_filterNO_filteredMeas.mat differ
diff --git a/results/simulations/battery/report/Carnevale_nofilter_noise_linevarying_pwm_N60.mat b/results/simulations/battery/report/Carnevale_nofilter_noise_linevarying_pwm_N60.mat
new file mode 100644
index 0000000..e337e8c
Binary files /dev/null and b/results/simulations/battery/report/Carnevale_nofilter_noise_linevarying_pwm_N60.mat differ
diff --git a/results/simulations/battery/report/Carnevale_nofilter_noise_linevarying_sin.mat b/results/simulations/battery/report/Carnevale_nofilter_noise_linevarying_sin.mat
new file mode 100644
index 0000000..15d4af9
Binary files /dev/null and b/results/simulations/battery/report/Carnevale_nofilter_noise_linevarying_sin.mat differ
diff --git a/results/simulations/battery/report/Carnevale_nofilter_noise_novarying_pwm.mat b/results/simulations/battery/report/Carnevale_nofilter_noise_novarying_pwm.mat
new file mode 100644
index 0000000..5ac567a
Binary files /dev/null and b/results/simulations/battery/report/Carnevale_nofilter_noise_novarying_pwm.mat differ
diff --git a/results/simulations/battery/report/Carnevale_nofilter_noise_novarying_sin.mat b/results/simulations/battery/report/Carnevale_nofilter_noise_novarying_sin.mat
new file mode 100644
index 0000000..2f5135b
Binary files /dev/null and b/results/simulations/battery/report/Carnevale_nofilter_noise_novarying_sin.mat differ
diff --git a/third.jpg b/third.jpg
new file mode 100644
index 0000000..e826e9d
Binary files /dev/null and b/third.jpg differ