% ECE 248, Kalman and Adaptive Filtering, Fall 2010, UCSB
% written by: Katie Byl, Sep 26, 2010
% edit list: 
%    Oct. 10, 2010 - minor changes, to post on the web
%
% Description:
%
% Multiple weight measurements were taken of a PUMPKIN (on a
% bathroom scale).  We start with an estimate of weight taken
% by assuming the pumpkin is a spherical "shell" and then
% refine this estimate recursively, as each new weight measurement
% is added in.  At the end, our estimate is IDENTICAL to what it
% would have been if we have averaged all estimates (including
% the volume-derived estimate) all at once, using appropriate
% weights (to account for better accuracy from the scale, vs
% the volume-based estimate).
%
% This recursive process capture the essential algorithm of
% Kalman filtering.

fprintf('This script recursively updates an estimate of pumpkin weight.\n')
fprintf('We start with an off-line estimate (based on volume).\n')
fprintf('Then we include one new (noisy) weight measurement at a time.\n')
format compact % single-spaced output
wt_off = 140+[1.4, 1.2, 1.0, 1.0, 0.8, 1.0, 1.0, 1.0, 1.0, 0.8];
wt_meas = 150+([1.6
    1.6
    1.8
    1.8
    2.2
    2.2
    1.8
    2.4
    1.8
    1.8
    1.8
    1.8
    1.8
    1.6
    2.0
    2.0
    1.8
    1.8
    2.0
    2.2
    1.8
    1.6
    1.6
    2.2
    2.2])-141;
mean_weight = mean(wt_meas)
std_weight = std(wt_meas)
R_scale = std_weight^2;

rpump = 31/(2*pi)*2.54;
thpump = 3;
vpump_out = 4/3*pi*(rpump)^3;
vpump_in = 4/3*pi*(rpump-thpump)^3;
Wguess = (vpump_out-vpump_in)/1000*2.2;

thpump2 = 2.5;  % another guess at thickness (1 SD off?)
vpump_in2 = 4/3*pi*(rpump-thpump2)^3; % corresponding 1SD off weight est
Wguess2 = (vpump_out-vpump_in2)/1000*2.2;
Pstd = Wguess - Wguess2
%Pstd = 1e6  % To essentially "ignore" the pre-estimate...
%Pstd = .2 % To give the pre-estimate a LOT of weight, make Pstd small
Q = 0*1e-6;  % Process noise. Could just make this zero. Weight should NOT change.
%R = R_scale^2 % 
R = .2^2; % resolution of the scale...

x_start = Wguess;  % This is our "best guess" at an initial value...
hF3 = figure(3);
clf; hold on
set(hF3,'NumberTitle','off')
set(hF3,'Name','Estimating a pumpkin''s weight')

nlist = 1:length(wt_meas);
xk_pre = zeros(1,length(wt_meas));
xk_pre(1) = x_start;  % no dynamics in system. Weight is a CONSTANT.
xk_post = 0*xk_pre;
Pk_pre = 0*xk_pre;
Pk_pre(1) = Pstd^2 + Q;
Pk_post = 0*Pk_pre;
Pk(1) = Pstd;
subplot(212)
p1 = semilogy(0,Pstd,'kd-','LineWidth',2,'MarkerFaceColor',[0 .7 0]);
axis([0 25 1e-2 5])
grid on
hold on
xlabel('Trial number'); ylabel('Variance (lb^2)')
subplot(211)
c_pumpkin = [.8 .4 0];
p2 = plot(0,x_start,'ko-','LineWidth',2,'MarkerFaceColor',c_pumpkin);
axis([0 25 10 11.5])
grid on
hold on
xlabel('Trial number'); ylabel('Pumpkin weight estimate (lb)')
title('Recursive estimates of weight, with error bars. x shows measurements.')

for n=1:length(wt_meas)
    if n>1
        % First, set the "pre-measurement" values.
        % Since we have no dynamics, there is no "process update"
        % actually going on (unless process variance Q is nonzero, below).
        xk_pre(n) = xk_post(n-1);  % step 1 in KF
        Pk_pre(n) = Pk_post(n-1)+Q;  % step 2 in KF
    end
    Kk = Pk_pre(n)/(Pk_pre(n)+R);  % step 3 in KF (Kalman weighting factor)
    xk_post(n) = xk_pre(n) + Kk*(wt_meas(n)-xk_pre(n));  % step 4 in KF (state)
    Pk_post(n) = Pk_pre(n)*(1-Kk);  % step 5 (of 5) in KF (variance)
    subplot(212)
    set(p1,'XData',[0:n],'YData',[Pstd, Pk_post(1:n).^.5]);
    subplot(211)
    plot(n,wt_meas(n),'bx','LineWidth',2,'MarkerSize',10)
    set(p2,'XData',[0:n],'Ydata',[x_start, xk_post(1:n)]);
    plot(n+[0 0],xk_post(n)+(Pk_post(n)^.5)*[-2 2],'r-') % 2-sigma error bars
    plot(n+[-1 1]*.1,xk_post(n)+(Pk_post(n)^.5)*2+[0 0],'r-') % error bar "top"
    plot(n+[-1 1]*.1,xk_post(n)+(Pk_post(n)^.5)*(-2)+[0 0],'r-') % error bar "bottom"
    input('Hit enter to continue...')
end

% Compare variance of iterative process, vs adding all at once:
final_std = Pk_post(end)^.5 
expected_std = (1 / (25*(1/R) + (1/Pstd)^2))^.5
difference_in_stds = final_std - expected_std

final_mean = xk_post(end)
expected_mean = (sum(wt_meas./R) + (xk_pre(1)/Pk_pre(1)))*expected_std^2
difference_in_means = final_mean - expected_mean
