function [Estimation_X0,FirstLandmarks] = FEKF_update(Estimation_X0, CameraMeasurementThis, Sigma_OB , FirstLandmarks)
NumberOfLandmarksObInThisStep = size(CameraMeasurementThis,1)/3;

% initialise the IndexOfFeature if possible
if size(Estimation_X0.landmarks,2) > 0
    IndexOfFeature = Estimation_X0.landmarks(4,:)';
else
    IndexOfFeature = [];
end

IndexObservedAlreadyThis = [];
IndexObservedNew = [];   
for i = 1:NumberOfLandmarksObInThisStep
    % check whether the feature is observed before or not
    M = find( IndexOfFeature== CameraMeasurementThis(3*i,2) );
    if isempty(M)
        IndexObservedNew = [IndexObservedNew;CameraMeasurementThis(3*i,2) ];
    else
        IndexObservedAlreadyThis = [IndexObservedAlreadyThis;CameraMeasurementThis(3*i,2)];
    end             
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% IndexObservedNew=[78; 97; 18] indicates that the 
% robot firstly observes landmarks 78 97 18 in this step
% IndexObservedAlreadyThis=[19; 20; 53] indicates 
% that the robot observes again landmarks 19 20 53 in this step
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


orientation = Estimation_X0.orientation;
position    = Estimation_X0.position;
cov         = Estimation_X0.cov;

NumberOfFeature = size( IndexOfFeature,1);
NumberOfOldFeatureInThisStep = size(IndexObservedAlreadyThis,1);
NumberOfNewFeatureInThisStep = size(IndexObservedNew,1);
 



% update state vector and covariance by considering 
% new feature into state and covariance
if ~isempty(IndexObservedNew)
    
    orientation = Estimation_X0.orientation;
    position    = Estimation_X0.position;
    
   % copy previous covariance
    temp    = repmat({eye(3)}, NumberOfNewFeatureInThisStep, 1 );
    tempKK  = blkdiag(temp{:});
    Sigma   = blkdiag(Estimation_X0.cov,tempKK);
    KK      = eye(6+3*(NumberOfFeature+NumberOfNewFeatureInThisStep));
    
 %   add new features
    for i = 1:NumberOfNewFeatureInThisStep
        indNewf = IndexObservedNew(i);
        Estimation_X0.landmarks(4,NumberOfFeature+i) = indNewf;
        m2 = find( CameraMeasurementThis(:,2) == indNewf );
        nf = CameraMeasurementThis( m2, 1 );

        Estimation_X0.landmarks(1:3,NumberOfFeature+i) = orientation*nf+position;
        Newlandmark=[ orientation*nf+position;  indNewf  ];
        
        FirstLandmarks=[FirstLandmarks  Newlandmark];
        
%         KK( 6+3*NumberOfFeature+3*i-2:6+3*NumberOfFeature+3*i,1:6 ) = [-skew(Estimation_X0.orientation*(nf)) eye(3)];  
%         KK (6+3*NumberOfFeature+3*i-2:6+3*NumberOfFeature+3*i, 6+3*NumberOfFeature+3*i-2:6+3*NumberOfFeature+3*i )=Estimation_X0.orientation;
        
         KK( 6+3*NumberOfFeature+3*i-2:6+3*NumberOfFeature+3*i,1:6 ) = [-skew(orientation*(nf)) eye(3)];  
        KK (6+3*NumberOfFeature+3*i-2:6+3*NumberOfFeature+3*i, 6+3*NumberOfFeature+3*i-2:6+3*NumberOfFeature+3*i )=orientation;
                tempKK(3*i-2:3*i,3*i-2:3*i)=diag(nf.^2)*Sigma_OB^2;
    end
    Sigma   = blkdiag(Estimation_X0.cov,tempKK);
    Estimation_X0.cov = KK*Sigma*KK';
       
    NumberOfFeature=NumberOfFeature+NumberOfNewFeatureInThisStep;
end


orientation = Estimation_X0.orientation;
position    = Estimation_X0.position;
cov         = Estimation_X0.cov;

   
% update state and covariance 
if ~isempty(IndexObservedAlreadyThis)
    Z = zeros( NumberOfOldFeatureInThisStep*3 , 1); 
    Y = zeros( NumberOfOldFeatureInThisStep*3 , 1); 
    H = zeros(3*NumberOfOldFeatureInThisStep, 6+3*NumberOfFeature);
    
    temp = repmat({eye(3)}, NumberOfOldFeatureInThisStep,1 );
    R = blkdiag(temp{:});
    
    
    % update old features
    for i = 1:NumberOfOldFeatureInThisStep
        ind = find(IndexOfFeature == IndexObservedAlreadyThis(i));
        
        fi  = Estimation_X0.landmarks(1:3,ind);
        Y(3*i-2:3*i,1) = ObservationModel( orientation, position, fi );
        
        ind2 = find(CameraMeasurementThis(:,2) == IndexObservedAlreadyThis(i));
        Z(3*i-2:3*i,1 ) = CameraMeasurementThis(ind2,1);
        
        FirstLandmark=FirstLandmarks(1:3,ind);
        
        H(3*i-2:3*i, 1:6) = [-orientation'*skew(FirstLandmark-position) orientation'];
        H(3*i-2:3*i, 6+3*ind-2:6+3*ind) = -orientation';    
        R(3*i-2:3*i,3*i-2:3*i) = diag(CameraMeasurementThis(ind2,1).^2)*Sigma_OB^2;
    end    
    
    % question @RomaTeng, different computaton scheme
    z = Z-Y;
    S = H*cov*H'+R;
    K = cov*H'*inv(S);
    s = K*z;
    
    Estimation_X0 = SpecialAdd(Estimation_X0,-s);
    cov = ( eye(6+3*NumberOfFeature) -K*H )*cov;
    Estimation_X0.cov = cov;
    
   
end  
     


     
     



