Tyler Bishop

EE 236 - Project 3

Due 03/24/23
Part 1
The data was loaded into matlab. The provided time vector was used to find the time
step between IMU acceleration measurements, and to verify that they were all identical.

Fig 1.1: Acceleration measurements for the first 20 seconds, when stationary. The
distribution appears to be roughly gaussian with a mean around -0.01.

Fig 1.2: Acceleration measurement for entire experiment. Red stars highlighting 20
second intervals where the sensor is stationary.
Part 2
A function was created to propagate the state according the dynamic model described
by equations 12-14, given an initial state and measurement data as input.

Fig 2.1: Integrated acceleration data using different biases. The integral line used a bias
calculated as the integral of the first 20 seconds of no movement, averaged over time, giving a
value of about 0.01. The average bias was calculated simply as the average of the first 20
seconds of data, this resulted in a near identical value. This makes sense as averaging is
similar to performing rectangular integration and dividing by the time interval. The best bias
estimate was the quadratic curve fit described in the project description.

Fig 2.2: State estimates from the integrated acceleration data for the entire duration. The
integration used the quadratic curve fit bias of around 0.08.
Part 3
The error model was derived using the state and estimate models provided in the project
description. As well as an adjusted error model including the scaling factor as a state variable.

Fig 3.1: Derivation of the state error model.

Fig 3.2: Derivation of the linearized model of acceleration measurement including a

scaling factor ‘s’.

Fig 3.3: Derivation of the modified state error model included the scaling factor.
Part 4

Fig 4.1: State variables position, velocity, and bias plotted over 19 separate intervals
spanning the first 20 seconds of the experiment. It can be seen that the position estimate tends
to run away, indicting accumulating error due to sensor bias. This is despite the recalculating of
the bias as the average of acceleration data over the interval.

Fig 4.2: Position and velocity trajectories plotted over the same interval. The red lines
correspond the propagated covariance of either state variable. Very roughly 68% of the
trajectories could be said to pass through the region defined by +/-sigma.
 Fig 4.3: Terms of the closed form covariance propagation. During the 1 second interval it
can be seen the n(t) variance dominates the other terms.
Part 5

Fig 5.1: Plot of Kalman filter state estimates over entire experiment duration in blue.
Estimated state +/- the square root (1 sigma) of the diagonals of the covariance matrix in black.

Fig 5.2: Kalman state estimates over 3 time intervals with different acceleration behavior. The
corrected values given by the estimate at k*20 seconds are within the bounds of the +/- sigma
curves, which would be expected about 68% of the time.
Appendix - Matlab Code
%%
clear
load("sim_data20170220A.mat")
%% part 1
clf
del_t = t(2) - t(1); % =.008 = 1/125 seconds
std(t(2:end) - t(1:end-1)); % deviation 0, all = 0.008s
n = length(acc);
T_20 = fix(20/del_t); % 20s w/ .008s intervals
% stationary for first 20 seconds
figure(1)
clf
plot(t(1:T_20),acc(1:T_20),'.')
ylabel("acc measurement")
xlabel("time(s)")
% attempting to highlight 20 second interval measurements
figure(2)
clf
plot(t,acc)
for k = T_20:T_20:n-T_20
hold on
plot(t(k-2:k+2),acc(k-2:k+2),'r*')
end
hold off
ylabel("acc measurement")
xlabel("time(s)")
%% part 2
x0 = zeros(3,1);
x_t = integrate_imu(n,acc,x0,del_t);
bias_I = (-1/20)*trapz(acc(1:T_20))*del_t; % integrate measurement data for bias
x0(3,1) = bias_I;
x_I = integrate_imu(n-T_20,acc(T_20:end),x0,del_t);
% fit model position error
bias_Q = 2*1428.5/(580^2);
x0(3,1) = bias_Q;
x_I2 = integrate_imu(n-T_20,acc(T_20:end),x0,del_t);
% average acceleration for bias
bias_A = -sum(acc(1:T_20)) / length(acc(1:T_20));
x0(3,1) = bias_A;
x_I3 = integrate_imu(n-T_20,acc(T_20:end),x0,del_t);
figure(1)
clf
hold on
plot(t,x_t(1,2:end))
plot(t(T_20:end),x_I(1,1:end),".-")
plot(t(T_20:end),x_I2(1,1:end))
plot(t(T_20:end),x_I3(1,1:end))
legend("No bias","Integral","Fit model","Average")
figure(2)
clf
hold on
subplot(311)
plot(t(T_20:end),x_I2(1,1:end))
xlabel("time(s)")
ylabel("position(m)")
subplot(312)
plot(t(T_20:end),x_I2(2,1:end))
xlabel("time(s)")
ylabel("velocity(m/s)")
subplot(313)
plot(t(T_20:end),x_I2(3,1:end))
xlabel("time(s)")
ylabel("bias(m/s/s)")
%% part 3
% no code
%% part 4
T_sec = fix(1/del_t) ; % time steps in one second
x_t = zeros(T_20-T_sec,4); % storing acceleration-bias input to make plotting easier
x_ts1 = zeros(T_sec+1,19);
x_ts2 = zeros(T_sec+1,19);
for s = 0:18
acc_int = acc(s*T_sec+1:(s+2)*T_sec);
bias_A = -sum(acc_int)/length(acc_int);
var_b = mean( (acc_int-bias_A).^2 ); % var = average of difference squared

xh_s_0 = [0 0 bias_A]';
x_t_int = integrate_imu(T_sec,acc_int(T_sec:end),xh_s_0,del_t);
x_t((s)*T_sec+1:(s+1)*T_sec+1,1:3) = x_t_int'; % store interval x est
x_t((s)*T_sec+1:(s+1)*T_sec+1,4) = acc_int(T_sec:end)'-bias_A;
x_ts1(:,(s+1)) = x_t_int(1,:)';
x_ts2(:,(s+1)) = x_t_int(2,:)';
end
std_b = std(x_t(:,3));
P_s_0 = diag([0 0 std_b^2]);
Q = [1e-6 0; 0 1e-10];
P_t = prop_cov(T_sec+1,P_s_0,del_t,Q); % use last bias variance
com_t = t(1:T_sec+1);
% error covar closed form
t_bar = 1:0.01:2;
sig_n = 1e-3; sig_w = 1e-5; sig_b = std_b;
Pp_t = [((1/3)*sig_n^2).*t_bar.^3 ;
((1/4)*sig_b^2).*t_bar.^4 ;
((1/20)*sig_w^2).*t_bar.^5];
Pv_t = [sig_n^2 .* t_bar ; sig_b^2 .* t_bar.^2 ; (1/3)*sig_w^2 .* t_bar.^3];
figure(1)
clf
hold on
subplot(411)
plot(t(T_sec:T_20),x_t(:,1),".")
xlabel("time(s)")
ylabel("position")
subplot(412)
plot(t(T_sec:T_20),x_t(:,2),".")
xlabel("time(s)")
ylabel("velocity")
subplot(413)
plot(t(T_sec:T_20),x_t(:,3),".")
xlabel("time(s)")
ylabel("bias")
subplot(414)
plot(t(T_sec:T_20),x_t(:,4),".")
xlabel("time(s)")
ylabel("a(t)-b_hat(t)")
sig_p = P_t(1,:).^(1/2);
sig_v = P_t(5,:).^(1/2);
figure(2)
clf
subplot(211)
hold on
title("position")
plot(com_t,x_ts1',"b")
plot(com_t,sig_p,"r",com_t,-sig_p,"r")
subplot(212)
hold on
title("velocity")
plot(com_t,x_ts2',"b")
plot(com_t,sig_v,"r",com_t,-sig_v,"r") % fifth compoenent = P22
figure(3)
clf
subplot(211)
plot(t_bar,Pp_t)
legend("n term","b term","w term")
subplot(212)
plot(t_bar,Pp_t)
legend("n term","b term","w term")
%% Part 5
nm = length(acc);
% calculate model coefficient matricies
phi = @(T) [1 T 0.5*T^2;0 1 T; 0 0 1];
gam = @(T) [.5*T^2 0 ; T 0 ;0 1 ];
H = [1 0 0]; % y is measurement of position
Yk = 0; % at all t=k*T sensor is stationary at origin, p=0, v=0?
Qd = [1e-6 0; 0 1e-10];
R = 0.01^2; %sig_eta squared
% preallocate arrays
x_est_t = zeros(3,nm);
P_diag_t = zeros(3,nm);
residual = zeros(2,fix(nm/T_20));
% initalize state estimates and covariance
P_k_p = diag([0 0 std_b^2]);
X_k_p = [0 0 bias_Q]'; % use bias with best result from part 2
for k = 2:nm
% state propagation update
uk = acc(k); % measurement minus bias
gam_k = gam(del_t);
Q_k = gam_k*Qd*gam_k';
[X_k_m,P_k_m] = KF_state(X_k_p, P_k_p, Q_k, phi(del_t), gam_k(:,1), uk);
% perform measurement update when position is known
if mod(k,T_20) == 0
% perform measurement update, pos=0
[X_k_p,P_k_p, res_k, res_std] = KF_measure(X_k_m, P_k_m, Yk, H, R);
residual(:,fix(k/T_20)) = [res_k;res_std];
else
X_k_p = X_k_m; % store prior in posterior variable for convencience
P_k_p = P_k_m;
end
x_est_t(:,k) = X_k_p;
P_diag_t(:,k) = diag(P_k_p);
end
disp("plotting")
t_span = [280,340];
figure(1)
clf
subplot(311)
hold on
plot(t,x_est_t(1,:),".")
plot(t,x_est_t(1,:)+P_diag_t(1,:).^(1/2),"k-") % plot curves for +/-sigma
plot(t,x_est_t(1,:)-P_diag_t(1,:).^(1/2),"k-")
xlabel("time(s)")
ylabel("position")
xlim(t_span)
subplot(312)
hold on
plot(t,x_est_t(2,:),".")
plot(t,x_est_t(2,:)+P_diag_t(2,:).^(1/2),"k-")
plot(t,x_est_t(2,:)-P_diag_t(2,:).^(1/2),"k-")
xlabel("time(s)")
ylabel("velocity")
xlim(t_span)
subplot(313)
hold on
plot(t,x_est_t(3,:),".")
plot(t,x_est_t(3,:)+P_diag_t(3,:).^(1/2),"k-")
plot(t,x_est_t(3,:)-P_diag_t(3,:).^(1/2),"k-")
xlabel("time(s)")
ylabel("bias")
xlim(t_span)
%% functions
function X = integrate_imu(n,u,x0,del_t)
% integrator for part 2 and 4
X = zeros(3,n+1);
X(:,1) = x0;
for i=2:n+1
pi=X(1,i-1);
vi=X(2,i-1);
bi=X(3,i-1);

ai=u(i-1)+bi; % expect bias to be provided as negative already

X(1,i) = pi + vi*del_t + 0.5*ai*del_t^2;

X(2,i) = vi+ai*del_t;
X(3,i) = bi;
end
end
function P_t = prop_cov(n,P0,del_T,Q_k)
T = del_T; % assumes constant dt and constant Q
phi = [1 T 0.5*T^2;0 1 T; 0 0 1];
gam = [.5*T^2 0 ; T 0 ;0 1 ];
P_t = zeros(9,n); % flatten matrix to store
P = P0;
P_t(:,1) = reshape(P,[9,1]);
for i = 2:n
P = phi*P*phi' + gam*Q_k*gam';
P_t(:,i) = reshape(P,[9,1]);
end
end
function [Xk_p,Pk_p, res_k, res_std] = KF_measure(Xk_m, Pk_m, Yk, H, R)
% update state with measurement at t(k)
K_k = Pk_m*H' * inv(H*Pk_m*H' + R)
Pk_p = Pk_m - K_k*H*Pk_m;

res_k = Yk-H*Xk_m;
res_std = sqrt(H*Pk_p*H' + R);
Xk_p = Xk_m + K_k*res_k;
%disp([K_k,Pk_m*H',[res_k;H*Pk_m*H';0]])
end
function [Xk_m, Pk_m] = KF_state(Xk_p, Pk_p, Q_k, phi_k,gam,uk)
% propogate state and covariance from t(k) to t(k+1)
Xk_m = phi_k*Xk_p + gam*uk ;
 Pk_m = phi_k*Pk_p*phi_k'+Q_k;
%disp([Pk_p Pk_m Q_k])
end