moorepants · July 4, 2025 12:06
diff --git a/kalman_filter.m b/kalman_filter.m
 function [x, P] = kalman_filter(A, B, C, Q, R, x, u, y, P)
 % kalman_filter - Returns the next state estimate and its covariance matrix
 % using the Kalman Filter predict & update. Note that this implementation is
 % not numerically stable, use production code for non-textbook problems.
 %
 % Syntax: [x, P] = kalman_filter(A, B, C, Q, R, x, u, y, P)
 %
 % Inputs:
 %   A - State matrix, size 5 x 5
 %   B - Input matrix, size 5 x 1
 %   C - Output matrix, 2 x 5
 %   Q - Model covariance matrix, size 5 x 5
 %   R - Measurement convariance matrix, size 2 x 2
 %   x - Current state, size 5 x 1
 %   u - Current input, size 1 x 1
 %   y - Current measurement, size 2 x 1
 %   P - Current state covariance matrix, size 5 x 5
 % Outputs:
 %   x - New state, size 5 x 1
 %   P - New state covariance matrix, 5 x 5

 x = A*x + B*u;
 P = A*P*A' + Q;
 L = P*C'*inv(C*P*C' + R);
 P = (eye(length(x)) - L*C)*P;
 x = x + L*(y - C*x);

 end
diff --git a/run_kalman_filter.m b/run_kalman_filter.m
 data = csvread('smartphone-data.csv', 1, 0);

 t = data(:, 1);
 v = data(:, 5);
 omega = data(:, 6);
 a = data(:, 7);
 theta_true = data(:, 8);
 v_true = data(:, 9);

 dt = % 1 x 1
 g = 9.81;

 A = % 5 x 5
 B = % 5 x 1
 C = % 2 x 5

 Q_scale = 1.0;  % adjust this number, model covariance
 R_scale = 1.0;  % adjust this number, measurement covariance

 Q = Q_scale*diag([0.0, 0.0, 0.0, 0.0, 1.0]);
 R = R_scale*diag([2.5e-01, 9.0e-04]);

 % initial P and x
 P = diag([1.0e+02, 2.5e-01, 1.0e-04, 4.0e-04, 9.0e-04]);
 x = % 5 x 1, initial state

 xs = zeros(length(?), 5);  % add correct length of time vector
 xs(1, :) = x;
 for i=2:length(?) % add correct length of time vector
    u = % 1 x 1, model input
    y = % 2 x 1, actual measurements
    % run the filter for this time step, updating x and P
    [x, P] = kalman_filter(A, B, C, Q, R, x, u, y, P);
    % store the estimated state vector
    xs(i, :) = x;
 end

 % make some plots
 figure();
 plot(t, v_true, t, v, '.', t, xs(:, 2));
 figure();
 plot(t, theta_true, t, xs(:, 4));
 figure();
 plot(t, omega, '.', t, xs(:, 5));
diff --git a/run_kalman_filter.py b/run_kalman_filter.py
 import numpy as np
 import matplotlib.pyplot as plt

 def kalman_filter(A, B, C, Q, R, x, u, y, P):
    """Returns the next state estimate and its covariance matrix.

    Parameters
    ==========
    A : shape(5, 5)
        State matrix
    B : shape(5, 1)
        Input matrix
    C : shape(2, 5)
        Output matrix
    Q : shape(5, 5)
        Model covariance matrix
    R : shape(2, 2)
        Measurement convariance matrix
    x : shape(5, 1)
        Current state
    u : shape(1, 1)
        Current input
    y : shape(2, 1)
        Current measurement
    P : shape(5, 5)
        Current state covariance matrix

    Returns
    =======
    x : shape(5, 1)
        New state
    P : shape(5, 5)
        New state covariance matrix

    """
    x = A@x + B@u
    P = A@[email protected] + Q
    L = [email protected]@np.linalg.inv(C@[email protected] + R)
    P = (np.eye(len(x)) - L@C)@P
    x = x + L@(y - C@x)
    return x, P


 data = np.loadtxt('smartphone-data.csv', skiprows=1, delimiter=',')

 t = data[:, 0];
 v = data[:, 4];
 omega = data[:, 5];
 a = data[:, 6];
 theta_true = data[:, 7];
 v_true = data[:, 8];

 dt = # 1 x 1

 A = # 5 x 5
 B = # 5 x 1
 C = # 2 x 5

 Q_scale = 1.0  # adjust this number, model covariance
 R_scale = 1.0  # adjust this number, measurement covariance

 Q = Q_scale*np.diag([0.0, 0.0, 0.0, 0.0, 1.0])
 R = R_scale*np.diag([2.5e-01, 9.0e-04])

 P = np.diag([1.0e+02, 2.5e-01, 1.0e-04, 4.0e-04, 9.0e-04])

 x = # 5 x 1, initial state

 us =  # n x 1 x 1, inputs
 ys =  # n x 2 x 1, measurements

 xs = np.zeros((len(us), 5, 1)) # n x 5 x 1
 for i, (u, y) in enumerate(zip(us, ys)):
    x, P = kalman_filter(A, B, C, Q, R, x, u, y, P)
    xs[i] = x

 # make some plots
 plt.figure()
 plt.plot(t, v_true, t, v, '.', t, xs[:, 1, 0]);

 plt.figure()
 plot(t, theta_true, t, xs[:, 3, 0]);

 plt.figure()
 plot(t, omega, '.', t, xs[:, 4, 0]);
diff --git a/smartphone-data.csv b/smartphone-data.csv
diff --git a/xtra_kalman.m b/xtra_kalman.m
 % load the data
 data = csvread('smartphone-data.csv', 1, 0);

 % extract the data into vectors
 t = data(:, 1);  % [s]
 zEL = data(:, 2);  % [m]
 lat = data(:, 3);  % [deg]
 lon = data(:, 4);  % [deg]
 v = data(:, 5);  % [m/s]
 omega = data(:, 6);  % [rad/s]
 a = data(:, 7);  % [m/s/s]
 theta_true = data(:, 8);  % [rad]
 v_true = data(:, 9);  % [m/s]

 % Exercise 3
 % convert lat & lon to radians
 lat_rad = lat*pi/180;
 lon_rad = lon*pi/180;
 % use equirectangular projection for xy in surface plane
 r = 6371000.0;  % radius of Earth [m]
 xEW = r*lon_rad*cos(mean(lat_rad));
 yNS = r*lat_rad;
 % calculate the distance along the path in the surface plane
 s = cumsum(sqrt(diff(xEW).^2 + diff(yNS).^2));

 % plot zEL vs s
 figure();
 plot(s/1000, zEL(2:end), '.');
 xlabel('s [km]');
 ylabel('zEL [m]');

 % calculate the slope and then the angle
 angle = atan(diff(zEL(2:end))./diff(s));

 % plot theta vs s
 figure();
 plot(s(2:end)/1000, angle*180/pi, '.', s/1000, theta_true(2:end)*180/pi);
 xlabel('s [km]');
 ylabel('theta [deg]');
 legend('GPS', 'True');

 % Exercise 4
 theta_from_int = cumtrapz(t, omega);
 figure();
 plot(t, theta_true, t, theta_from_int);
 xlabel('t [s]');
 ylabel('theta [deg]');
 legend('True', 'From integration');

 % Exercise 5
 dt = mean(diff(t));
 g = 9.81;

 A = [1.0,  dt, 0.0, -0.5*g*dt^2, 0.0;
     0.0, 1.0, 0.0,       -g*dt, 0.0;
     0.0, 0.0, 0.0,          -g, 0.0;
     0.0, 0.0, 0.0,         1.0,  dt;
     0.0, 0.0, 0.0,         0.0, 1.0];

 B = [0.5*dt^2;
     dt;
     1.0;
     0.0;
     0.0];

 C = [0.0, 1.0, 0.0, 0.0, 0.0;
     0.0, 0.0, 0.0, 0.0, 1.0];

 Q_scale = 1.0;
 R_scale = 1.0;

 Q = Q_scale*diag([0.0, 0.0, 0.0, 0.0, 1.0]);
 R = R_scale*diag([2.5e-01, 9.0e-04]);

 % initial P and x
 P = diag([1.0e+02, 2.5e-01, 1.0e-04, 4.0e-04, 9.0e-04]);
 x = [0.0; v(1); 0.0; 0.0; omega(1)];

 xs = zeros(length(t), length(x));
 xs(1, :) = x;
 for i=2:length(t)
    u = a(i);
    y = [v(i); omega(i)];
    [x, P] = kalman_filter(A, B, C, Q, R, x, u, y, P);
    xs(i, :) = x;
 end

 % plot Kalman filter results
 figure();
 plot(t, v_true, t, v, '.', t, xs(:, 2));
 xlabel('t [s]');
 ylabel('v [m/s]');
 legend('True', 'Measured', 'Kalman');
 figure();
 plot(t, theta_true, t, xs(:, 4));
 xlabel('t [s]')
 ylabel('theta [rad]');
 legend('True', 'Kalman');
 figure();
 plot(t, omega, '.', t, xs(:, 5));
 xlabel('t [s]')
 ylabel('omega [rad/s]');
 legend('Measured', 'Kalman');
	function [x, P] = kalman_filter(A, B, C, Q, R, x, u, y, P)
	% kalman_filter - Returns the next state estimate and its covariance matrix
	% using the Kalman Filter predict & update. Note that this implementation is
	% not numerically stable, use production code for non-textbook problems.
	%
	% Syntax: [x, P] = kalman_filter(A, B, C, Q, R, x, u, y, P)
	%
	% Inputs:
	% A - State matrix, size 5 x 5
	% B - Input matrix, size 5 x 1
	% C - Output matrix, 2 x 5
	% Q - Model covariance matrix, size 5 x 5
	% R - Measurement convariance matrix, size 2 x 2
	% x - Current state, size 5 x 1
	% u - Current input, size 1 x 1
	% y - Current measurement, size 2 x 1
	% P - Current state covariance matrix, size 5 x 5
	% Outputs:
	% x - New state, size 5 x 1
	% P - New state covariance matrix, 5 x 5

	x = Ax + Bu;
	P = APA' + Q;
	L = PC'inv(CPC' + R);
	P = (eye(length(x)) - LC)P;
	x = x + L(y - Cx);

	end
	data = csvread('smartphone-data.csv', 1, 0);

	t = data(:, 1);
	v = data(:, 5);
	omega = data(:, 6);
	a = data(:, 7);
	theta_true = data(:, 8);
	v_true = data(:, 9);

	dt = % 1 x 1
	g = 9.81;

	A = % 5 x 5
	B = % 5 x 1
	C = % 2 x 5

	Q_scale = 1.0; % adjust this number, model covariance
	R_scale = 1.0; % adjust this number, measurement covariance

	Q = Q_scale*diag([0.0, 0.0, 0.0, 0.0, 1.0]);
	R = R_scale*diag([2.5e-01, 9.0e-04]);

	% initial P and x
	P = diag([1.0e+02, 2.5e-01, 1.0e-04, 4.0e-04, 9.0e-04]);
	x = % 5 x 1, initial state

	xs = zeros(length(?), 5); % add correct length of time vector
	xs(1, :) = x;
	for i=2:length(?) % add correct length of time vector
	u = % 1 x 1, model input
	y = % 2 x 1, actual measurements
	% run the filter for this time step, updating x and P
	[x, P] = kalman_filter(A, B, C, Q, R, x, u, y, P);
	% store the estimated state vector
	xs(i, :) = x;
	end

	% make some plots
	figure();
	plot(t, v_true, t, v, '.', t, xs(:, 2));
	figure();
	plot(t, theta_true, t, xs(:, 4));
	figure();
	plot(t, omega, '.', t, xs(:, 5));
	import numpy as np
	import matplotlib.pyplot as plt

	def kalman_filter(A, B, C, Q, R, x, u, y, P):
	"""Returns the next state estimate and its covariance matrix.

	Parameters
	==========
	A : shape(5, 5)
	State matrix
	B : shape(5, 1)
	Input matrix
	C : shape(2, 5)
	Output matrix
	Q : shape(5, 5)
	Model covariance matrix
	R : shape(2, 2)
	Measurement convariance matrix
	x : shape(5, 1)
	Current state
	u : shape(1, 1)
	Current input
	y : shape(2, 1)
	Current measurement
	P : shape(5, 5)
	Current state covariance matrix

	Returns
	=======
	x : shape(5, 1)
	New state
	P : shape(5, 5)
	New state covariance matrix

	"""
	x = A@x + B@u
	P = A@[email protected] + Q
	L = [email protected]@np.linalg.inv(C@[email protected] + R)
	P = (np.eye(len(x)) - L@C)@P
	x = x + L@(y - C@x)
	return x, P


	data = np.loadtxt('smartphone-data.csv', skiprows=1, delimiter=',')

	t = data[:, 0];
	v = data[:, 4];
	omega = data[:, 5];
	a = data[:, 6];
	theta_true = data[:, 7];
	v_true = data[:, 8];

	dt = # 1 x 1

	A = # 5 x 5
	B = # 5 x 1
	C = # 2 x 5

	Q_scale = 1.0 # adjust this number, model covariance
	R_scale = 1.0 # adjust this number, measurement covariance

	Q = Q_scale*np.diag([0.0, 0.0, 0.0, 0.0, 1.0])
	R = R_scale*np.diag([2.5e-01, 9.0e-04])

	P = np.diag([1.0e+02, 2.5e-01, 1.0e-04, 4.0e-04, 9.0e-04])

	x = # 5 x 1, initial state

	us = # n x 1 x 1, inputs
	ys = # n x 2 x 1, measurements

	xs = np.zeros((len(us), 5, 1)) # n x 5 x 1
	for i, (u, y) in enumerate(zip(us, ys)):
	x, P = kalman_filter(A, B, C, Q, R, x, u, y, P)
	xs[i] = x

	# make some plots
	plt.figure()
	plt.plot(t, v_true, t, v, '.', t, xs[:, 1, 0]);

	plt.figure()
	plot(t, theta_true, t, xs[:, 3, 0]);

	plt.figure()
	plot(t, omega, '.', t, xs[:, 4, 0]);
	% load the data
	data = csvread('smartphone-data.csv', 1, 0);

	% extract the data into vectors
	t = data(:, 1); % [s]
	zEL = data(:, 2); % [m]
	lat = data(:, 3); % [deg]
	lon = data(:, 4); % [deg]
	v = data(:, 5); % [m/s]
	omega = data(:, 6); % [rad/s]
	a = data(:, 7); % [m/s/s]
	theta_true = data(:, 8); % [rad]
	v_true = data(:, 9); % [m/s]

	% Exercise 3
	% convert lat & lon to radians
	lat_rad = lat*pi/180;
	lon_rad = lon*pi/180;
	% use equirectangular projection for xy in surface plane
	r = 6371000.0; % radius of Earth [m]
	xEW = rlon_radcos(mean(lat_rad));
	yNS = r*lat_rad;
	% calculate the distance along the path in the surface plane
	s = cumsum(sqrt(diff(xEW).^2 + diff(yNS).^2));

	% plot zEL vs s
	figure();
	plot(s/1000, zEL(2:end), '.');
	xlabel('s [km]');
	ylabel('zEL [m]');

	% calculate the slope and then the angle
	angle = atan(diff(zEL(2:end))./diff(s));

	% plot theta vs s
	figure();
	plot(s(2:end)/1000, angle180/pi, '.', s/1000, theta_true(2:end)180/pi);
	xlabel('s [km]');
	ylabel('theta [deg]');
	legend('GPS', 'True');

	% Exercise 4
	theta_from_int = cumtrapz(t, omega);
	figure();
	plot(t, theta_true, t, theta_from_int);
	xlabel('t [s]');
	ylabel('theta [deg]');
	legend('True', 'From integration');

	% Exercise 5
	dt = mean(diff(t));
	g = 9.81;

	A = [1.0, dt, 0.0, -0.5gdt^2, 0.0;
	0.0, 1.0, 0.0, -g*dt, 0.0;
	0.0, 0.0, 0.0, -g, 0.0;
	0.0, 0.0, 0.0, 1.0, dt;
	0.0, 0.0, 0.0, 0.0, 1.0];

	B = [0.5*dt^2;
	dt;
	1.0;
	0.0;
	0.0];

	C = [0.0, 1.0, 0.0, 0.0, 0.0;
	0.0, 0.0, 0.0, 0.0, 1.0];

	Q_scale = 1.0;
	R_scale = 1.0;

	Q = Q_scale*diag([0.0, 0.0, 0.0, 0.0, 1.0]);
	R = R_scale*diag([2.5e-01, 9.0e-04]);

	% initial P and x
	P = diag([1.0e+02, 2.5e-01, 1.0e-04, 4.0e-04, 9.0e-04]);
	x = [0.0; v(1); 0.0; 0.0; omega(1)];

	xs = zeros(length(t), length(x));
	xs(1, :) = x;
	for i=2:length(t)
	u = a(i);
	y = [v(i); omega(i)];
	[x, P] = kalman_filter(A, B, C, Q, R, x, u, y, P);
	xs(i, :) = x;
	end

	% plot Kalman filter results
	figure();
	plot(t, v_true, t, v, '.', t, xs(:, 2));
	xlabel('t [s]');
	ylabel('v [m/s]');
	legend('True', 'Measured', 'Kalman');
	figure();
	plot(t, theta_true, t, xs(:, 4));
	xlabel('t [s]')
	ylabel('theta [rad]');
	legend('True', 'Kalman');
	figure();
	plot(t, omega, '.', t, xs(:, 5));
	xlabel('t [s]')
	ylabel('omega [rad/s]');
	legend('Measured', 'Kalman');