close all;
clear;
clc;
%% Script for combined localization simulation
%% Simulation Data
n = 1000;
target_rad = 0.0015;
sensors = ones(n,8);
Temp_Var=0.00075; % temp var of 0.75 is too high --> fix!
Q = 0.005;

%test case 1
% IR
% sensors(:,1) = 1*sensors(:,1) + Q*randn(n,1);
% sensors(:,2) = 1*sensors(:,2)+ Q*randn(n,1);
% sensors(:,3) = 0.05*sensors(:,3)+ Q*randn(n,1);
% sensors(:,4) = 1*sensors(:,4)+ Q*randn(n,1);
% % Thermocouple
% sensors(:,5)= (randn(n,1)*Temp_Var)+linspace(0,6,n)';
% sensors(:,6)= (randn(n,1)*Temp_Var)+linspace(0,2,n)';
% sensors(:,7)= (randn(n,1)*Temp_Var)+linspace(0,2,n)';
% sensors(:,8)= (randn(n,1)*Temp_Var)+linspace(0,2,n)';
% %test case 2
% % IR
% sensors(:,1) = 1*sensors(:,1) + Q*randn(n,1);
% sensors(:,2) = 0.05*sensors(:,2)+ Q*randn(n,1);
% sensors(:,3) = 1*sensors(:,3)+ Q*randn(n,1);
% sensors(:,4) = 0.02*sensors(:,4)+ Q*randn(n,1);
% % Thermocouple
% sensors(:,5)= (randn(n,1)*Temp_Var)+linspace(0,2,n)';
% sensors(:,6)= (randn(n,1)*Temp_Var)+linspace(0,2,n)';
% sensors(:,7)= (randn(n,1)*Temp_Var)+linspace(0,2,n)';
% sensors(:,8)= (randn(n,1)*Temp_Var)+linspace(0,6,n)';
% %
%test case 3
% IR
sensors(:,1) = 1*sensors(:,1) + Q*randn(n,1);
sensors(:,2) = 0.02*sensors(:,2)+ Q*randn(n,1);
sensors(:,3) = 0.02*sensors(:,3)+ Q*randn(n,1);
sensors(:,4) = 1*sensors(:,4)+ Q*randn(n,1);
% Thermocouple
sensors(:,5)= (randn(n,1)*Temp_Var)+linspace(0,2,n)';
sensors(:,6)= (randn(n,1)*Temp_Var)+linspace(0,6,n)';
sensors(:,7)= (randn(n,1)*Temp_Var)+linspace(0,2,n)';
sensors(:,8)= (randn(n,1)*Temp_Var)+linspace(0,2,n)';

%Get source location from sim data
Sum_sensors=(sensors(:,5)+sensors(:,6)+sensors(:,7)+sensors(:,8));
filt_source_location(:,1)=.5+(9-((sensors(:,5)+sensors(:,6))./Sum_sensors(:)).*9);
filt_source_location(:,2)=.5+(((sensors(:,5)+sensors(:,7))./Sum_sensors(:)).*9);

true_sum=(6+2+2+2);
true_location(1)=0.5+(9-((2+6)/(true_sum))*9);
true_location(2)=0.5+(((2+2)/(true_sum))*9);

figure()
hold on
c = linspace(1,n,length(Sum_sensors));
scatter(filt_source_location(:,1),filt_source_location(:,2),[100],c)
xlim([0 10])
ylim ([0 10])
colorbar;

%% Bayesian Fusion

% User inputs
size_x = 0.1; % length of plate [m]
size_y = 0.1; % width of plate [m]
grid_x = 10; %number of cells in x-directions
grid_y = grid_x; %number of cells in y-directions
target_rad = 0.015;

% IR info
sensor_pos = [1  3; 1 8 ; 2 3 ; 2 8]; % ([x(1),y(2)], position) line of sight

% Thermocouple info

% Occupancy Grid
map = robotics.OccupancyGrid(0.1,0.1,grid_x*10);
[x,y] = meshgrid(1:grid_x);
setOccupancy(map,[x(:) y(:)],0.5, "grid");

% Initialize grid probabilities
lk_odds = zeros(grid_x,grid_y,5);
pk_occ = 0.5*ones(grid_x,grid_y,5);
p_occ = 0.5*ones(grid_x,grid_y); % overall grid

lmin = -2.0;
lmax = 3.5;
measured = 0;

dx = size_x/grid_x;
dy = size_y/grid_y;

delta_t = 0.01;
tau = 0.1;

for t=1:size(sensors,1)
    t;
    for i=1:5
        %only for IR
        if i < 5
            % Inverse sensor model
            cur_pos = sensor_pos(i,:);

            % Convert volt to distance using sensor model
            cur_meas = sensors(t,i)+target_rad;
            mu = cur_meas;
            sigma = 0.005;
        else
            % Bivariate Gaussian distribution
            % mu = current estimate position of heat source
            mu = filt_source_location(i,:)/100;
            sigma = [0.0025,0.0025];
        end

        for j=1:grid_x
            for k=1:grid_y
                xy = grid2world(map,[j k]);

                if i < 5
                    % Guassian inverse sensor model
                    % mean at object detection distance
                    % variance is sensor noise
                    % if current grid cell is in the line of sight update
                    % probability
                    if (cur_pos(1) == 1 && cur_pos(2) == j)
                        prob_inv = 0.8*(normpdf(xy(1),mu,sigma)/normpdf(mu,mu,sigma))+0.1;
                        if xy(1) > mu && prob_inv < 0.5
                            prob_inv = 0.5;
                        end
                        l = lk_odds(j,k,i) + log10(prob_inv/(1-prob_inv));
                    elseif (cur_pos(1) == 2 && cur_pos(2) == k)
                        prob_inv = 0.8*(normpdf(xy(2),mu,sigma)/normpdf(mu,mu,sigma))+0.1;
                        if xy(2) > mu && prob_inv < 0.5
                            prob_inv = 0.5;
                        end
                        l = lk_odds(j,k,i) + log10(prob_inv/(1-prob_inv));
                    else
                        prob_inv = 0.5;
                        l = lk_odds(j,k,i);
                    end
                else
                    %apply forgetting factor
                    pk_occ(j,k,i) = (pk_occ(j,k,i)-0.5)*exp(-delta_t/tau)+0.5;
                    lk_odds(j,k,i) = log10(pk_occ(j,k,i)/(1-pk_occ(j,k,i)));

                    %bivariate normal distribution
                    prob_inv = 0.8*(mvnpdf(xy,mu,sigma)/mvnpdf(mu,mu,sigma))+0.1;
                    l = lk_odds(j,k) + log10(prob_inv/(1-prob_inv));
                end
                % log odds update (temporal)
                if l > lmax
                    l = lmax;
                elseif l < lmin
                    l = lmin;
                end
                pk_occ(j,k,i) = prob_inv;
                lk_odds(j,k,i) = l;
            end
        end
    end
    % Multiple sensor fusion (grid)
    for h=1:size(pk_occ,1)
        for j=1:size(pk_occ,2)
            cur_lodds = sum(lk_odds(h,j,:));
            prob = 1-(1/(1+exp(cur_lodds)));
            p_occ(h,j) = prob;
        end
    end
end
% Grid update from a measurement
[y,x] = meshgrid(1:grid_x);
setOccupancy(map,[x(:) y(:)],p_occ(:), "grid");
figure()
show(map);
hold on
% Get xy values of grid with probability of 0.8 or higher
x = [];
y = [];
for i=1:size(p_occ,1)
    for j=1:size(p_occ,2)
        if p_occ(i,j) >= 0.7
            xy = grid2world(map,[i j]);
            x = vertcat(x,xy(1));
            y = vertcat(y,xy(2));
        end
    end
end
x_loc = mean(x);
y_loc = mean(y);
plot(x_loc,y_loc,'xr','MarkerSize',10)
grid on;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Script for combined localization with experimental data
close all;
clear;
clc;
%% File import

filenames = "combined_test_case_2.xlsx";
raw_data = xlsread(filenames);
%raw_data(:,1)=[];


%% Simulation Data
target_rad = 0.0015;
Temp_Var=0.75; % temp var of 0.75 is too high --> fix!
Q = 0.005;

% IR
s1_offset = 0.06; %short
s2_offset = 0.06; %short
s3_offset = 0.24; %medium
s4_offset = 0.24; %long

% Converting voltage to distance and removing offset
sensors(:,1) = 10.83*raw_data(:,2).^-1.060/100 - s1_offset;
sensors(:,2) = 10.83*raw_data(:,3).^-1.060/100 - s2_offset;
sensors(:,3) = 21.88*raw_data(:,4).^-1.055/100 - s3_offset;
sensors(:,4) = 10.83*raw_data(:,5).^-1.060/100 - s4_offset;

sensors(sensors(:,1:4) > 0.15) = 1.0;
sensors(sensors(:,1:4) < 0) = 0;
% Thermocouple
sensors(:,5) = ((raw_data(:,6)-1.25)./0.005)-23;
sensors(:,6) = ((raw_data(:,7)-1.25)./0.005)-23;
sensors(:,7) = ((raw_data(:,8)-1.25)./0.005)-23;
sensors(:,8) = ((raw_data(:,9)-1.25)./0.005)-23;

sensors(1:end-1000,:)=[];

%% Kalman filtering data
R=[0.00003,0.00003,0.00003,0.00003]; %Based off test case 3
Q=0.0001*ones(1,4); %Assume low noise
F=[1,1,1,1]; %Assume little change
Pc=[0,0,0,0];
G=[0,0,0,0];
P=[0,0,0,0];
Xp=[0,0,0,0];
Zp=[0,0,0,0];
Xe=[0,0,0,0];
filt_temperatures=zeros(length(sensors(:,1)),4);

for i = 1:length(sensors(:,1))
    Pc=P+Q;
    G= Pc./(Pc+R); %Kalman gain
    P=(1-G).*Pc;
    Xp=Xe;
    Zp=Xp;
    Xe=G.*(sensors(i,5:8)-Zp)+Xp; %Kalman estimate
    filt_temperatures(i,:)=Xe;
end

sensors(:,5)=filt_temperatures(:,1);
sensors(:,6)=filt_temperatures(:,2);
sensors(:,7)=filt_temperatures(:,3);
sensors(:,8)=filt_temperatures(:,4);


filt_temp_sum=(sensors(:,5)+sensors(:,6)+sensors(:,7)+sensors(:,8));
filt_source_location(:,1)=.5+(((sensors(:,5)+sensors(:,7))./filt_temp_sum(:)).*9);
filt_source_location(:,2)=.5+(9-((sensors(:,5)+sensors(:,6))./filt_temp_sum(:)).*9);

%%
figure()
hold on
c = linspace(1,length(filt_temp_sum),length(filt_temp_sum));
scatter(filt_source_location(:,1),filt_source_location(:,2),[10],c)
xlim([0 10])
ylim ([0 10])
colorbar;

%% Bayesian Fusion

% User inputs
size_x = 0.1; % length of plate [m]
size_y = 0.1; % width of plate [m]
grid_x = 10; %number of cells in x-directions
grid_y = grid_x; %number of cells in y-directions
target_rad = 0.015;

% IR info
sensor_pos = [1  3; 1 8 ; 2 3 ; 2 8]; % ([x(1),y(2)], position) line of sight

% Occupancy Grid
map = robotics.OccupancyGrid(0.1,0.1,grid_x*10);
[x,y] = meshgrid(1:grid_x);
setOccupancy(map,[x(:) y(:)],0.5, "grid");

% Initialize grid probabilities
lk_odds = zeros(grid_x,grid_y,5);
pk_occ = 0.5*ones(grid_x,grid_y,5);
p_occ = 0.5*ones(grid_x,grid_y); % overall grid

lmin = -2.0;
lmax = 3.5;
measured = 0;

dx = size_x/grid_x;
dy = size_y/grid_y;

delta_t = 0.001;
tau = 0.1;
%%
for t=1:size(sensors,1)
    t;
    for i=1:5
        %only for IR
        if i < 5
            % Inverse sensor model
            cur_pos = sensor_pos(i,:);

            % Convert volt to distance using sensor model
            cur_meas = sensors(t,i)+target_rad;
            mu = cur_meas;
            sigma = 0.005;
        else
            % Bivariate Gaussian distribution
            % mu = current estimate position of heat source
            mu = filt_source_location(i,:)/100;
            sigma = [0.0025,0.0025];
        end

        for j=1:grid_x
            for k=1:grid_y
                xy = grid2world(map,[j k]);

                if i < 5
                    % Guassian inverse sensor model
                    % mean at object detection distance
                    % variance is sensor noise
                    % if current grid cell is in the line of sight update
                    % probability
                    if (cur_pos(1) == 1 && cur_pos(2) == j)
                        prob_inv = 0.8*(normpdf(xy(1),mu,sigma)/normpdf(mu,mu,sigma))+0.1;
                        if xy(1) > mu && prob_inv < 0.5
                            prob_inv = 0.5;
                        end
                        l = lk_odds(j,k,i) + log10(prob_inv/(1-prob_inv));
                    elseif (cur_pos(1) == 2 && cur_pos(2) == k)
                        prob_inv = 0.8*(normpdf(xy(2),mu,sigma)/normpdf(mu,mu,sigma))+0.1;
                        if xy(2) > mu && prob_inv < 0.5
                            prob_inv = 0.5;
                        end
                        l = lk_odds(j,k,i) + log10(prob_inv/(1-prob_inv));
                    else
                        prob_inv = 0.5;
                        l = lk_odds(j,k,i);
                    end
                else
                    %apply forgetting factor
                    pk_occ(j,k,i) = (pk_occ(j,k,i)-0.5)*exp(-delta_t/tau)+0.5;
                    lk_odds(j,k,i) = log10(pk_occ(j,k,i)/(1-pk_occ(j,k,i)));

                    %bivariate normal distribution
                    prob_inv = 0.8*(mvnpdf(xy,mu,sigma)/mvnpdf(mu,mu,sigma))+0.1;
                    l = lk_odds(j,k) + log10(prob_inv/(1-prob_inv));
                end
                % log odds update (temporal)
                if l > lmax
                    l = lmax;
                elseif l < lmin
                    l = lmin;
                end
                pk_occ(j,k,i) = prob_inv;
                lk_odds(j,k,i) = l;
            end
        end
    end
    % Multiple sensor fusion (grid)
    for h=1:size(pk_occ,1)
        for j=1:size(pk_occ,2)
            cur_lodds = sum(lk_odds(h,j,:));
            prob = 1-(1/(1+exp(cur_lodds)));
            p_occ(h,j) = prob;
        end
    end
end
% Grid update from a measurement
[y,x] = meshgrid(1:grid_x);
setOccupancy(map,[x(:) y(:)],p_occ(:), "grid");
figure()
show(map);
hold on
% Get xy values of grid with probability of 0.8 or higher
x = [];
y = [];
for i=1:size(p_occ,1)
    for j=1:size(p_occ,2)
        if p_occ(i,j) >= 0.7
            xy = grid2world(map,[i j]);
            x = vertcat(x,xy(1));
            y = vertcat(y,xy(2));
        end
    end
end
x_loc = mean(x);
y_loc = mean(y);
plot(x_loc,y_loc,'xr','MarkerSize',10)
grid on;
%%%%%%%%%%%%%%%
%Code for thermal localization simulation
close all;
clear;
clc;

%% Experiment Data
filenames = "combined_test_case_1.xlsx";
raw_data = xlsread(filenames);
raw_data(:,1:5)=[];

%% Bayesian Fusion

% User inputs
size_x = 0.1; % length of plate [m]
size_y = 0.1; % width of plate [m]
grid_x = 10; %number of cells in x-directions
grid_y = grid_x; %number of cells in y-directions
num_ir = 4;
target_rad = 0.015;
sensor_pos = [1  3; 1 8 ; 2 3 ; 2 8]; % ([x(1),y(2)], position) line of sight

% Occupancy Grid
map = robotics.OccupancyGrid(0.1,0.1,grid_x*10);
[x,y] = meshgrid(1:grid_x);
setOccupancy(map,[x(:) y(:)],0.5, "grid");

% Initialize grid probabilities
p_occ = 0.5*ones(grid_x,grid_y); % overall grid
l_odds = log10(p_occ./(1-p_occ));

lmin = -2.0;
lmax = 3.5;
measured = 0;

dx = size_x/grid_x;
dy = size_y/grid_y;

delta_t = 0.001;
tau = 0.001;
%% Simulation data
Temp_Var=0.75; %Temperature variance

Sim_Temp_Data_1=[(randn(60000,1)*Temp_Var)+linspace(0,6,60000)'...
    (randn(60000,1)*Temp_Var)+linspace(0,2,60000)' (randn(60000,1)*Temp_Var)+linspace(0,2,60000)' (randn(60000,1)*Temp_Var)+linspace(0,2,60000)']

Sim_Temp_Data_2=[(randn(60000,1)*Temp_Var)+linspace(0,2,60000)'...
    (randn(60000,1)*Temp_Var)+linspace(0,2,60000)' (randn(60000,1)*Temp_Var)+linspace(0,2,60000)' (randn(60000,1)*Temp_Var)+linspace(0,6,60000)']

Sim_Temp_Data_3=[(randn(60000,1)*Temp_Var)+linspace(0,2,60000)'...
    (randn(60000,1)*Temp_Var)+linspace(0,6,60000)' (randn(60000,1)*Temp_Var)+linspace(0,2,60000)' (randn(60000,1)*Temp_Var)+linspace(0,2,60000)']

% Sim_Temp_Data_4=[(randn(60000,1)*Temp_Var)+linspace(0,2,60000)'...
%     (randn(60000,1)*Temp_Var)+linspace(0,2,60000)' (randn(60000,1)*Temp_Var)+linspace(0,6,60000)' (randn(60000,1)*Temp_Var)+linspace(0,2,60000)']
%
% Sim_Temp_Data_5=[(randn(60000,1)*Temp_Var)+linspace(0,2,60000)'...
%     (randn(60000,1)*Temp_Var)+linspace(0,6,60000)' (randn(60000,1)*Temp_Var)+linspace(0,6,60000)' (randn(60000,1)*Temp_Var)+linspace(0,2,60000)']
%
% Sim_Temp_Data_6=[(randn(60000,1)*Temp_Var)+linspace(0,6,60000)'...
%     (randn(60000,1)*Temp_Var)+linspace(0,6,60000)' (randn(60000,1)*Temp_Var)+linspace(0,2,60000)' (randn(60000,1)*Temp_Var)+linspace(0,2,60000)']

%Get source location from sim data
Sum_Sim_Temp_Data_1=(Sim_Temp_Data_3(:,1)+Sim_Temp_Data_3(:,2)+Sim_Temp_Data_3(:,3)+Sim_Temp_Data_3(:,4));
filt_source_location(:,1)=.5+9-(((Sim_Temp_Data_3(:,1)+Sim_Temp_Data_3(:,2))./Sum_Sim_Temp_Data_1(:)).*9);
filt_source_location(:,2)=.5+((Sim_Temp_Data_3(:,1)+Sim_Temp_Data_3(:,3))./Sum_Sim_Temp_Data_1(:)).*9;

%%
%for t=1:size(filt_source_location,1)
for t=1:1000
    t;
    % Bivariate Gaussian distribution
    % mu = current estimate position of heat source
    mu = filt_source_location(t,:)/100;
    sigma = [0.0025,0.0025];
    for j=1:grid_x
        for k=1:grid_y
            %apply forgetting factor
            p_occ(j,k) = (p_occ(j,k)-0.5)*exp(-delta_t/tau)+0.5;
            l_odds(j,k) = log10(p_occ(j,k)/(1-p_occ(j,k)));

            xy = grid2world(map,[j k]);
            prob_inv = 0.8*(mvnpdf(xy,mu,sigma)/mvnpdf(mu,mu,sigma))+0.1;
            l = l_odds(j,k) + log10(prob_inv/(1-prob_inv));

            % log odds update (temporal)
            if l > lmax
                l = lmax;
            elseif l < lmin
                l = lmin;
            end
            l_odds(j,k) = l;

            prob = 1-(1/(1+exp(l_odds(j,k))));
            p_occ(j,k) = prob;

        end
    end

end

[y,x] = meshgrid(1:grid_x);
setOccupancy(map,[x(:) y(:)],p_occ(:), "grid");
show(map);
hold on
% Get xy values of grid with probability of 0.8 or higher
x = [];
y = [];
for i=1:size(p_occ,1)
    for j=1:size(p_occ,2)
        if p_occ(i,j) >= 0.7
            xy = grid2world(map,[i j]);
            x = vertcat(x,xy(1));
            y = vertcat(y,xy(2));
        end
    end
end
x_loc = mean(x);
y_loc = mean(y);
plot(x_loc,y_loc,'xr','MarkerSize',10)
grid on;
colorbar;

%%
figure()
hold on
c = linspace(1,60000,length(Sum_Sim_Temp_Data_1));
scatter(filt_source_location(:,1),filt_source_location(:,2),[100],c)
xlim([0 10])
ylim ([0 10])
colorbar;

x_moving_Variance=movvar(filt_source_location(:,1),600);
y_moving_Variance=movvar(filt_source_location(:,2),600);

figure()
hold on
plot(x_moving_Variance)
plot(y_moving_Variance)
plot(1:1:60000,0.75*ones(60000,1))
ylim([0 4])
title('Change in variance over time with 600 point window')
xlabel('Time step (0.001s)')
ylabel('Variance [cm]')
%%%%%%%%%%%%%
% IR Localization simulation

close all;
clear;
clc;

%% Simulation Data
n = 100;
sensors = ones(n,4);
Q = 0.005;

%test case 1
% sensors(:,1) = 1*sensors(:,1) + Q*randn(n,1);
% sensors(:,2) = 0.01*sensors(:,2)+ Q*randn(n,1);
% sensors(:,3) = 0.01*sensors(:,3)+ Q*randn(n,1);
% sensors(:,4) = 1*sensors(:,4)+ Q*randn(n,1);

%test case 2
sensors(:,1) = 1*sensors(:,1) + Q*randn(n,1);
sensors(:,2) = 1*sensors(:,2)+ Q*randn(n,1);
sensors(:,3) = 0.035*sensors(:,3)+ Q*randn(n,1);
sensors(:,4) = 1*sensors(:,4)+ Q*randn(n,1);

%test case 3
% sensors(:,1) = 0.07*sensors(:,1) + Q*randn(n,1);
% sensors(:,2) = 1*sensors(:,2)+ Q*randn(n,1);
% sensors(:,3) = 1*sensors(:,3)+ Q*randn(n,1);
% sensors(:,4) = 0.07*sensors(:,4)+ Q*randn(n,1);

%% Experiment Data
%filenames = "ir_test_case_2.xlsx";
filenames = "combined_test_case_1.xlsx";
raw_data = xlsread(filenames);

% figure;
% t = raw_data(:,1);
% for i = 2:5
%     plot(t,raw_data(:,i));
%     hold on;
% end
% legend();

s1_offset = 0.06; %short
s2_offset = 0.06; %short
s3_offset = 0.24; %medium
s4_offset = 0.24; %long

% Converting voltage to distance and removing offset
n = 1000;
sensors = zeros(n,4);
sensors(:,1) = 10.83*raw_data(1:n,2).^-1.060/100 - s1_offset;
sensors(:,2) = 10.83*raw_data(1:n,3).^-1.060/100 - s2_offset;
sensors(:,3) = 21.88*raw_data(1:n,4).^-1.055/100 - s3_offset;
sensors(:,4) = 53.37*raw_data(1:n,5).^-1.14/100 - s4_offset;

sensors(sensors > 0.15) = 1.0;
sensors(sensors < 0) = 0;
%%
% figure;
% t = raw_data(:,1);
% for i = 1:4
%     plot(t(1:100),sensors(:,i)');
%     hold on;
% end
% legend();

%% Bayesian Fusion

% User inputes
size_x = 0.1; % length of plate [m]
size_y = 0.1; % width of plate [m]
grid_x = 10; %number of cells in x-directions
grid_y = grid_x; %number of cells in y-directions
num_ir = 4;
target_rad = 0.015;
sensor_pos = [1  3; 1 8 ; 2 3 ; 2 8]; % ([x(1),y(2)], position) line of sight

% Occupancy Grid
map = robotics.OccupancyGrid(0.1,0.1,grid_x*10);
map.OccupiedThreshold = 0.7;
map.FreeThreshold = 0.3;

% Initialize grid probabilities
l_odds = zeros(grid_x,grid_y,size(sensors,2));
pk_occ = 0.5*ones(grid_x,grid_y,size(sensors,2));
p_occ = 0.5*ones(grid_x,grid_y); % overall grid

lmin = -2.0;
lmax = 3.5;
measured = 0;

dx = size_x/grid_x;
dy = size_y/grid_y;

delta_t = 0.1;
tau = 1;

for t=1:size(sensors,1)
    t
    for i=1:size(sensors,2)
        %i
        % Inverse sensor model
        cur_pos = sensor_pos(i,:);

        % Convert volt to distance using sensor model
        cur_meas = sensors(t,i)+target_rad;
        mu = cur_meas;
        sigma = 0.005;

        for j=1:grid_x
            for k=1:grid_y
                xy = grid2world(map,[j k]);

                % Guassian inverse sensor model
                % mean at object detection distance
                % variance is sensor noise

                % if current grid cell is in the line of sight update
                % probability
                if (cur_pos(1) == 1 && cur_pos(2) == j)
                    %apply forgetting factor
                    prob_inv = 0.8*(normpdf(xy(1),mu,sigma)/normpdf(mu,mu,sigma))+0.1;
                    if xy(1) >= mu && prob_inv < 0.5
                        prob_inv = 0.5;
                    end
                    l = l_odds(j,k,i) + log10(prob_inv/(1-prob_inv));
                elseif (cur_pos(1) == 2 && cur_pos(2) == k)
                    %apply forgetting factor
                    prob_inv = 0.8*(normpdf(xy(2),mu,sigma)/normpdf(mu,mu,sigma))+0.1;
                    if xy(2) >= mu && prob_inv < 0.5
                        prob_inv = 0.5;
                    end
                    l = l_odds(j,k,i) + log10(prob_inv/(1-prob_inv));
                else
                    prob_inv = 0.5;
                    l = l_odds(j,k,i);
                end
                pk_occ(j,k,i) = prob_inv;

                % log odds update (temporal)
                if l > lmax
                    l = lmax;
                elseif l < lmin
                    l = lmin;
                end
                l_odds(j,k,i) = l;
            end
        end
    end
    % Multiple sensor fusion (grid)
    for h=1:size(p_occ,1)
        for j=1:size(p_occ,2)
            cur_lodds = sum(l_odds(h,j,:));
            p_occ(h,j) = 1-(1/(1+exp(cur_lodds)));
        end
    end
    % Grid update from a measurement
    [y,x] = meshgrid(1:grid_x);
    setOccupancy(map,[x(:) y(:)],p_occ(:), "grid");
    show(map);
end
% Grid update from a measurement
[y,x] = meshgrid(1:grid_x);
setOccupancy(map,[x(:) y(:)],p_occ(:), "grid");
show(map);
hold on
% Get xy values of grid with probability of 0.8 or higher
x = [];
y = [];
for i=1:size(p_occ,1)
    for j=1:size(p_occ,2)
        if p_occ(i,j) >= 0.7
            xy = grid2world(map,[i j]);
            x = vertcat(x,xy(1));
            y = vertcat(y,xy(2));
        end
    end
end
x_loc = mean(x);
y_loc = mean(y);
plot(x_loc,y_loc,'xr','MarkerSize',10)
grid on;
colormap;

%% figures

%test case 1
t = 0:0.01:0.99;
sensors = ones(100,4);
sensors(:,1) = 0.2*sensors(:,1) + Q*randn(n,1);
sensors(:,2) = 0.01*sensors(:,2)+ Q*randn(n,1);
sensors(:,3) = 0.01*sensors(:,3)+ Q*randn(n,1);
sensors(:,4) = 0.2*sensors(:,4)+ Q*randn(n,1);
plot(t,sensors);
xlabel('Time [s]');
ylabel('Distance [m]');
%%
% inverse sensor model
d = 0:0.001:0.1;
sigma = 0.005;
norm = 0.8*(normpdf(d,0.05,sigma)/normpdf(0.05,0.05,sigma))+0.1;
norm(norm < 0.5) = 0.5;
norm1 = 0.8*(normpdf(d,0.05,sigma)/normpdf(0.05,0.05,sigma))+0.1;
norm1(51:101) = norm(51:101);
plot(d,norm1)
xlabel('Distance [m]');
ylabel('p(m_{ij}|d)');
grid on;
colorbar;