function [x_est y_est P] = augmented_UKF_weights(xh,ym,P,u,par)  
% This version augments the sigmapoints with one extra point
% and uses different tunings parameters

%% testing parameters
% clear all; clc;
% xh=1.0e+003 *[3.094006512137398;0.217298779318834;8.607929942130586];
% ym=[600 1000 300]'; 
% P=eye(3,3);
% 
% z1=0.4;    % production choke opening (u1) 
% z2=0.8;    % injection valve opening(u2) 
% % Disturbances
% GOR = 0;                     % Mass GOR from reservoir, NB: MASS, increasing gives: lower BH, stable
% P_r = 160;                      % Rservoir pressure [bar]
% P_gs = 150;                     % Gass source pressure [bar]
% P0 = 15;                        % Separator Pressure [bar]
% d = [GOR;P_r;P_gs;P0];         % Disturbance Vector
% u=[z1;z2;d]; %Only u(1) and u(2) are MVs
% par = v1_gaslift_parameters();

%% Solver Options
M=eye(3);               % Mass Matrix for ODE-solver
dT=1;                   % Time for Inegration 
T_span = [0 dT];        % Integrate from time=0 to dT
options=odeset('AbsTol',1e-10,'RelTol',1e-10,'MaxStep',100,'Mass',M);

%%
format('long')

%% Dimenion Parameters
%[w_Ga_in wmix_r_out alpha_lt p_ab/1e5 p_at/1e5 p_tt/1e5 p_bh/1e5 w_mix_r_out];  %Olga outputs
olga_sensors=[2 3];  % Availabe Measurements in OLGA model
% y=[w_G_a_in;P_a_t;P_r_t;P_a_b;P_bh;w_mix_r_out;rho_M_t;alpha_L_t]; Matlab outputs
matlab_sensors=[2 3]; % Available Measurements in Gaslift Model
%senors=[5];
Nx = length(xh);          % Number of States
Ns = (2*Nx) +1;              % Number of Sigma Points
Ny = length(olga_sensors);           % Number of Measured Measurements

%% Noise (uncertainty)
%V= 1e-4*eye(2);  % Measurement Noise tuning, no noise
V = 0.967798873798523*eye(2); %Tuning White Noise
a=1;
b=1;
c=1;
W=1e-5*[10 -a b; -a 10 -c; b -c 10];

%% Scaling Parameters
betta=2; % Tuning Parameter must be non-negavtive, introduced to affect the weighting of the zeroth sigmapoint for covariance calculaton
alpha=1e-3;%1; % Tuning Parameter must be 0<=alpha<=1 , should be small number, controls the size og the sigma-point distribution
kappa=0;%1; % Tuning Paramerer must be >=0

%%
lambda=(alpha^2)*(Nx+kappa) - Nx;
gamma=sqrt(Nx+lambda);

%% Weights
wm0 = lambda/(Nx+lambda);
wm = ones(2*Nx,1)*(1/(2*(Nx+lambda)));
wc = ones(2*Nx +1,1)*((lambda/(Nx+lambda)) + (1 -(alpha^2) +betta));
for i=2:1:length(wc)
    wc(i)=(1/(2*(Nx+lambda)));
end
%% Start UKF Algorithm

%% Calculate Sigma Points
[S0, valid] = chol(P);
if valid >0 % P is not positive definitt, can not use chol
    S=sqrt(abs(P));
else
    S=S0;
end


Xs=zeros(Nx,Ns);
for i=1:1:(Nx+1)    
    if i==1
        Xs(:,i)= xh;
    else
        Xs(:,i)   = xh + gamma*S(i-1,:)';
        Xs(:,i+Nx) = xh - gamma*S(i-1,:)';
    end
end


%% Apply Constraints on sigma Points
%Xs=sigma_constrain(Xs);

%% Propegate Sigma Points through System
Xs_prop=zeros(Nx,Ns);
 for i=1:1:Ns
    [t,state]=ode15s(@v1_gaslift_model,T_span,Xs(:,i),options,u,'derivatives',par);
    state=state(end,:);
    Xs_prop(:,i) = state; 
 end
%Xs_prop=sigma_constrain(Xs_prop);

%% Calculate apriori state estimate
 %X_ap=zeros(Nx,Ns);
 xh=zeros(Nx,1);
 for i=1:1:Ns
     if i==1
         xh = xh +wm0*Xs_prop(:,i);
     else
        xh = xh +wm(i-1)*Xs_prop(:,i);
     end
 end

%% Calculate the apriori covariance of states
P=zeros(Nx,Nx);
for i=1:1:Ns
    P= P + wc(i)*( (Xs_prop(:,i) -xh)*(Xs_prop(:,i) -xh)' );
end
P=P+W;
%% Get Measurements from Propegated sigmapoints

Ys=zeros(Ny,Ns);
for i=1:1:Ns
    y_full=v1_gaslift_model([],Xs_prop(:,i),u,'measurements',par);
    y=y_full(matlab_sensors);
    Ys(:,i)=y;
end

%% Calculate Means of Measurements
yh=zeros(Ny,1);
for i=1:1:Ns
    if i==1
        yh= yh +wm0*Ys(:,i);
    else
        yh= yh + wm(i-1)*Ys(:,i);
    end
end

%% Calculate the measurement Covariance

Py=zeros(Ny,Ny);
for i=1:1:Ns
    Py = Py + (wc(i)*( (Ys(:,i) -yh)*(Ys(:,i) -yh)' ));
end
Py=Py+V;
Py

%% Calculate the state measurement cross Covariance
Pxy=zeros(Nx,Ny);
for i=1:1:Ns
    Pxy= Pxy + wc(i)*((Xs_prop(:,i)-xh)*(Ys(:,i)-yh)');
end

%% Calculate Kalman Gain
K=Pxy/Py;

%% Update Covariance
P= P - K*Py*K'; 

%% Calculate Posteori Estmate
x_est= xh + K*(ym(olga_sensors) -yh);
y_est_full=v1_gaslift_model([],x_est,u,'measurements',par);
y_est=y_est_full;


end

