1. EENG 519: Final Project Report
Daniel Kuntz
Department of Electrical Engineering and Computer Science
Colorado School of Mines
Golden, Colorado
Email: dkuntz@mines.edu
Abstract—This paper describes the implementation an Un-
scented Kalman Filter in order to estimate the dynamics of a
balancing robot. Both simulation results and results obtained
from datasets provided in class are included as well as a
conclusion describing the effectiveness of the technique.
I. INTRODUCTION
In this project we are using an Unscented Kalman filter
to estimate the state of the balancing robot shown in Figure
I. The internal and output states of the robot are given by
equations (1) and (2). The continuous time dynamics models,
including terms for the length uncertainty and accelerometer
and gyroscope drift is given by (3) and (4). Table I has a
summary of the constant terms.
Fig. 1. Balancing Robot Sketch
˙x =






˙θ
˙ω
˙∆l
˙da
˙dg






= f(x, u) + wk (1)
y =
a
r
= g(x, u) + vk (2)
TABLE I
SUMMARY OF CONSTANT TERMS
constant description
θ angle of the robot from upright
ω angular velocity of the robot
∆l correction term for l
da drift of the accelerometer
dg drift of the gyroscope
a accelerometer reading
r gyroscope reading
g gravitational constant
m1 mass of the upper part of the robot
m2 mass of the lower part of the robot
l nominal distance from m1 to m2
la distance from base to accelerometer
u input force
f(x, u) =






ω
(g(m1+m2)−m1(l+∆l) cos(θ)ω2
) sin(θ)+cos(θ)u
(l+∆l)(m1 sin2(θ)+m2)
0
0
0






(3)
g(x, u) =
(l+∆l)−la
(l+∆l)
[cos(θ)−m1(l+∆l) cos(θ)ω2
sin θ]u+(m1+m2)g sin(θ)
m1 sin2(θ)+m2
˙θ
+
da
dg
(4)
The noise term wk represents state noise, this is needed
for the drift terms because they are modelled by a random
walk, it is not required for the other terms. However, adding a
small amount of noise to all states results in better numerical
stability. The measurement noise term vk is given, so we let:
wk ∼ N










0
0
0
0
0





, diag










10−6
10−6
10−6
10−4
Ts
10−4
Ts















(5)
vk ∼ N
0
0
,
10−4
0
0 10−4 (6)

2. II. METHODS
A. Discrete Approximation of System
To estimate the state of this system as it develops in time, the
Unscented Kalman Filter will performance will be compared
to that of the Extended Kalman Filter (not described in this
paper). To use these filters we first need a discrete time
representation of the system. This is simple in the case of
the measurement equation g(x, u):
yk = gd(xk, uk) = g(x(kTs), u(kTs)) (7)
For the state equations an Euler approximation can be used,
such as in (8), or an ODE solver in MATLAB can be utilized
to get more precise results. In particular, the coarse ”ode23”
solver works well. It was found that the much slower ode23
calculation does not provide much more accurate results, so
the Euler approximation was used in this project.
xk+1 = fd(xk, uk) = x(kTs) + f(x(kTs), u(kTs))Ts (8)
B. Unscented Kalman Updates
Once these equations have been established an a-priori
distribution representing our initial guess at the state of the
system is decided upon. A good guess is that the robot is in
equilibrium with states known to a variance of 0.1. Thus the
distribution representing our initial guess is:
ˆx0 ∼ N












0
0
0
0
0






,






0.1 0 0 0 0
0 0.1 0 0 0
0 0 0.1 0 0
0 0 0 0.1 0
0 0 0 0 0.1












(9)
Now we can begin updating this distribution as we get more
measurements with our filter. The idea with the Unscented
Kalman filter is to pick a set of points Xk at each time step
k with the weighting terms given by (14).
Xk,(i) =



xk i = 0
xk + (N + λ)Mk ˆni i = 1, · · · , N
xk − (N + λ)Mk ˆni i = N + 1, · · · , 2N
(10)
Where Mk is the Cholesky factorization of Pk. A summary
of the terms in (10) is given in Table II-B.
Once this set of points is determined it can be put into the
time update (11). Then, the weighted mean and covariance of
the resultant points is the same as the regular Kalman filter
update would be if fd happened to be linear.
X
(−)
k+1 = fd(X
(+)
k , uk) (11)
The weighted mean and covariance are then given by:
ˆx
(−)
k+1 =
2N
i=0
wµ
i X
(−)
k+1,(i) (12)
TABLE II
UNSCENTED WEIGHTING TERMS
term description value
N dimension of xk 5
α spread coefficient 10−3
β distribution coefficient 2
κ scaling factor 0
ˆni standard unit basis vector i -
λ = α2(N + κ) − N -
P
(−)
k+1 =
2N
i=0
wC
i (X
(−)
k+1,(i) − ˆx
(−)
k+1)(X
(−)
k+1,(i) − ˆx
(−)
k+1)T
(13)
Where:
wµ
0 = λ/(λ + N) (i = 0) (14)
wµ
i = 0.5/(λ + N) (i > 0)
wC
0 = wµ
0 + 1 − α2
+ β (i = 0)
wC
i = 0.5/(λ + N) (i > 0)
Similarly, the measurement update can be calculated with a
similar technique. First find a new set of points X
(−)
k by the
sampling technique given in (10). Then the update is:
Yk = g(X
(−)
k , uk) (15)
ˆyk =
2N
i=0
wµ
i Yk,(i) (16)
(Pyy)k =
2N
i=0
wC
i (Yk,(i) − ˆyk)(Yk,(i) − ˆyk)T
+ R (17)
(Pxy)k =
2N
i=0
wC
i (X
(−)
k,(i) − ˆx
(−)
k )(Yk,(i) − ˆyk)T
(18)
Kk = (Pxy)k(Pyy)−1
k (19)
ˆx
(+)
k = ˆx
(−)
k + Kk(yk − ˆyk) (20)
P
(+)
k = P
(−)
k − Kk(Pyy)kKT
k (21)
Continuing this for each time step gives us the Unscented
Kalman filter estimate. The MATLAB code implementing this
algorithm is given in Appendix A.

3. III. RESULTS
A. Simulation
The robot described in Section I was implemented in
Simulink, for which the block diagram is shown in Figure 2.
Simulations were run with the robot initially in the downward
position (θ = π) so that the displacements were reasonable
when the input force (22) was applied.
u(t) = sin( g/lt) (22)
Fig. 2. Balancing robot Simulink model
Also, the length of the robot was changed 10% to 0.418 m.
This information not being available to the UKF in order to
determine how well it guesses the uncertainty. The results of
this investigation show good performance. Figure 3 shows the
values that the filter tracks, Figure 4 shows the development
of the model variance for each state and Figure 5 shows the
error in the estimated and actual states. These plots show that
the filter was implemented properly and demonstrates that it
easily estimates the length of the uncertain robot model.
Fig. 3. UKF Simulation Results
Fig. 4. UKF Simulation State Variance
Fig. 5. UKF Simulation Estimation Error
B. Datasets
The next step is to test the filter on data provided with an
unknown input. Two datasets were provided for evaluation of
the filter. The first (dataset1.mat) did not have any change to
the length of the robot. The second, had a 10%, unspecified
length change that is to be estimated with our filter.
1) Dataset 1: The UKF estimates of the states are given in
6, and the variance for each state in the filter is given by 7.

4. Fig. 6. Dataset 1 state estimate
Fig. 7. Dataset 1 state variance
2) Dataset 2: The results for Dataset 2 are reported simi-
larly to Dataset 1 in Figures 8 and 9.
IV. CONCLUSIONS
Simulation results show that the filter works well and can
accurately estimate states, uncertainties in the model and drift
noise in the measurements. However, results obtained from
the provided datasets show no clear estimate of the true robot
length. This is seen for both the UKF and the EKF that it was
compared with. In fact, the two different filters agreed on the
real length of the robot almost perfectly.
It appears that the reason for this poor estimate of the real
length is due to the fact that the robot is not oscillating as much
in the provided dataset as it is in the simulation. This smaller
oscillation is not allowing the robot to see as much of the
effect of the change in length that the simulation results see.
This makes it difficult for the filter to accurately estimate the
length. It is clear from this that more accurate methods might
Fig. 8. Dataset 2 state estimate
Fig. 9. Dataset 2 state variance
need to be employed, such as a Monte-Carlo simulation with
a large number of samples.
REFERENCES
[1] T. Vincent, Course Notes for EENG 519, Estimation Theory and Kalman
Filtering, Colorado School of Mines, Spring Semester, 2015
[2] R. Brown, P. Hwang, Introduction to Random Signals and Applied
Kalman Filtering, 4th Edition, 2012 John Wiley & Sons
[3] MATLAB and Simulink Release 2014a, The MathWorks, Inc., Natick,
Massachusetts, United States.

5. APPENDIX A
MATLAB CODE
A. Model Initialization
%% Model Parameters for Balancing Robot
clear all;
close all;
%% Indicate initialization
is_init = 1;
%% Constants
g = 9.81; % m/sˆ2
m1 = 0.6; % kg
m2 = 1.9; % kg
l = 0.38; % m
la = 0.2; % m
dl_actual = l*0.1;
%% Sampling Rate
freq_s = 100; % Hz
t_s = 1/freq_s; % seconds
%% Noise Parameters
gyro_noise_var = .0001;
gyro_drift_var = .0001*t_s;
accel_noise_var = .0001;
accel_drift_var = .0001*t_s;
%% Inital condition
inital_ang = 0;
inital_vel = 0;
u = 0;
%% Save values to mat file
save('params');
B. Unscented Filter
function [x_est, P_est] = unscented_kalman_filter(y_meas, u_meas, t, x0, P0, Q, R)
%unscented_kalman_filter Returns the state estimation for the given data
%and input for the balancing robot model.
% Setup variables
sys_n = size(x0,1);
steps = size(t,2);
xk = zeros(sys_n, steps);
Pk = zeros(sys_n, sys_n, steps);
out_n = 2;
% get weights
[lambda, w_a, w_c] = get_unscented_weights(sys_n, 0.001);
for k = 1:size(t,2)
if (k > 1)
xk_minus = xk1_minus;
Pk_minus = Pk1_minus;
else
xk_plus = x0;
Pk_plus = P0;
xk(:,k) = x0;
Pk(:,:,k) = P0;
end
uk = u_meas(k);
yk = y_meas(:,k);
if (k > 1)
% Measurement update
unscented_xk = get_unscented_points(xk_minus, Pk_minus, lambda);

6. unscented_meas = gd(unscented_xk, uk);
unscented_meas_avg = w_avg(unscented_meas, w_a);
Pxy = avg_cov(unscented_xk, xk_minus, unscented_meas, unscented_meas_avg, w_c);
Py = avg_cov(unscented_meas, unscented_meas_avg, unscented_meas, unscented_meas_avg, w_c) + R;
% Calculate Kalman Gain
Kk = Pxy*inv(Py);
% Measurement update
xk_plus = xk_minus + Kk*(yk - unscented_meas_avg);
xk(:,k) = xk_plus;
Pk_plus = Pk_minus - Kk*Pxy';
Pk(:,:,k) = Pk_plus;
end
% Time update
unscented_xk = get_unscented_points(xk_plus, Pk_plus, lambda);
unscented_xk1 = fd(unscented_xk, uk);
unscented_xk1_avg = w_avg(unscented_xk1, w_a);
xk1_minus = unscented_xk1_avg;
Pk1_minus = avg_cov(unscented_xk1, unscented_xk1_avg, unscented_xk1, unscented_xk1_avg, w_c) + Q;
end
x_est = xk;
P_est = Pk;
end
C. Unscented Point Calculation
function pnts = get_unscented_points(xk, Pk, lambda)
%get_unscented_points Gets points for unscented Kalman update
% constants
sys_n = size(xk,1);
num_samp = sys_n*2 + 1;
M = chol(Pk, 'lower');
I = eye(sys_n);
pnts = zeros(sys_n, num_samp);
pnts(:,1) = xk;
for l = 1:sys_n
ll = l+1;
pnts(:,ll) = xk + sqrt(sys_n + lambda)*M*I(:,l);
end
for l = 1:sys_n
ll = l + sys_n + 1;
pnts(:,ll) = xk - sqrt(sys_n + lambda)*M*I(:,l);
end
end
D. Unscented Weight Calculation
function [lambda, w_avg, w_cov] = get_unscented_weights(sys_n, alpha)
%get_unscented_weights Calculates weights averaging unscented points
% constants
if nargin < 2
alpha = 0.001;
end
num_samp = sys_n*2 + 1;
beta = 2;
kappa = 0; %3*sys_n;
lambda = alphaˆ2*(sys_n + kappa) - sys_n;
% calculate weights
w_avg = zeros(1,num_samp);
w_cov = zeros(1,num_samp);
w_avg(1) = lambda/(lambda + sys_n);
w_cov(1) = w_avg(1) + 1 - alphaˆ2 + beta;
for l = 1:sys_n

7. ll = l+1;
w_avg(ll) = 1/(2*(lambda + sys_n));
w_cov(ll) = 1/(2*(lambda + sys_n));
end
for l = 1:sys_n
ll = l + sys_n + 1;
w_avg(ll) = 1/(2*(lambda + sys_n));
w_cov(ll) = 1/(2*(lambda + sys_n));
end
end
E. Discrete Dynamics
function xk1 = fd(xk, uk)
%fd Calculates and returns robot state at time k+1 given it's current state
% Load parameters
load('params');
num_pnts = size(xk, 2);
xk1 = zeros(5, num_pnts);
for n = 1:num_pnts
% Current values
ang = xk(1, n);
vel = xk(2, n);
dl = xk(3, n);
da = xk(4, n);
dg = xk(5 ,n);
% Calculate new values
ang1 = ang + vel*t_s;
acc = robot_dynamics(xk(:,n), uk, 0);
vel1 = vel + acc*t_s;
xk1(:,n) = [ang1; vel1; dl; da; dg];
end
end
F. Measurement
function yk = gd(xk, uk)
%gd Returns sensor measurements from the given robot state and input
num_pnts = size(xk, 2);
yk = zeros(2, num_pnts);
for n = 1:num_pnts
acc = accelerometer_sim(xk(:,n), uk, 0, 0);
gyr = gyroscope_sim(xk(:,n), 0);
yk(:,n) = [acc; gyr];
end
end
G. Robot Dynamics
function acc = robot_dynamics(state, input, dl_known)
%robot_dynamics Returns the acceleration of the robot given the angle,
%angular velocity and force input to the robot
% Get parameters
load('params');
ang = state(1);
vel = state(2);

8. u = input(1);
if dl_known
dl = dl_actual;
else
dl = state(3);
end
% calculate robot angular acceleration
ca = cos(ang);
sa = sin(ang);
acc = ((g*(m1 + m2) - m1*(l+dl)*ca*velˆ2)*sa + u*ca)/((l+dl)*(m1*saˆ2 + m2));
end
H. Accelerometer Simulation
function [reading, bias] = accelerometer_sim(state, input, noise_on, dl_known)
%accelerometer_sim Returns the simulated output of the accelerometer
% Declare Persistent drift term
persistent accel_drift;
% Get parameters
load('params');
% Unpack variables
ang = state(1);
vel = state(2);
if dl_known
dl = dl_actual;
else
dl = state(3);
end
u = input;
ca = cos(ang);
sa = sin(ang);
% calculate actual acceleration
term1 = ((l+dl) - la)/(l+dl);
term2 = (ca - m1*(l+dl)*ca*(velˆ2)*sa)*u;
term3 = (m1 + m2)*g*sa;
denom = (m1*saˆ2 + m2);
acc = term1*((term2 + term3)/denom);
% add noise
noise = sqrt(accel_noise_var)*randn();
if isempty(accel_drift)
accel_drift = 0;
end
if noise_on
accel_drift = accel_drift + sqrt(accel_drift_var)*randn();
reading = acc + noise + accel_drift;
else
est_drift = state(4);
reading = acc + est_drift;
end
bias = accel_drift;
end
I. Gyroscope Simulation
function [reading, bias] = gyroscope_sim(state, noise_on)
%gyroscope_sim Returns the simulated reading of the gyroscope
% create persistent drift variable
persistent gyro_drift;
% Get parameters

9. load('params');
vel = state(2);
% add noise
noise = sqrt(gyro_noise_var)*randn();
if isempty(gyro_drift)
gyro_drift = 0;
end
if noise_on
gyro_drift = gyro_drift + sqrt(gyro_drift_var)*randn();
reading = vel + noise + gyro_drift;
else
est_drift = state(5);
reading = vel + est_drift;
end
bias = gyro_drift;
end
J. Weighted Average Calculation
function x_avg = w_avg( x, w )
%w_avg Weighted average of the array x
sys_n = size(x,1);
tot = zeros(sys_n, 1);
for k = 1:size(x,2);
tot = tot + w(k)*x(:,k);
end
x_avg = tot;
K. Weighted Covariance Calculation
function P = avg_cov(xs, xavg, ys, yavg, w)
%avg_cov Calculates the average covariance of a set of vectors.
num = size(xs,2);
dim1 = size(xs,1);
dim2 = size(ys,1);
tot_cov = zeros(dim1, dim2);
for k = 1:num
tot_cov = tot_cov + w(k)*((xs(:,k) - xavg)*(ys(:,k) - yavg)');
end
P = tot_cov;