This document presents the design and implementation of two state estimators for a segway-like balancing robot using Kalman filtering techniques: the Extended Kalman Filter (EKF) and the Unscented Kalman Filter (UKF). The study analyzes their performance on various datasets, highlighting the impact of noise in state and measurements on estimating the robot's states, particularly the angle and angular velocity. Results indicate that both EKF and UKF effectively track the robot's state, though bias estimates exhibit some discontinuities.

1. Kalman Filtering Final Project
Surya Sekhar Chandra
Electrical Engineering
Colorado School of Mines
Golden, CO 80401
Email: schandra@mymail.mines.edu
Jonathan Pritchard
Electrical Engineering
Colorado School of Mines
Golden, CO 80401
Email: jpritcha@mymail.mines.edu
Abstract—This report details the design and implementation
of two state estimators for a Segway-like balancing robot. The
robot model is subject to noise in both state and measurement
and is inherently non-linear. Thus, a more sophisticated Kalman
Filtering approach must be used estimate the states of the system.
Both the Extended Kalman Filter and the Unscented Kalman
Filter are implemented and tested on several datasets pertaining
to the system. The results of both models on each dataset are
analyzed to determine the if either the EKF or UKF provides a
better way of modeling the system.
I. INTRODUCTION
This project focuses on determining the states of a Segway-
like balancing robot through state estimation. The robot bal-
ances on two wheels driven by DC motors which apply a
torque u at the base of the robot. The robot is modeled as two
masses, m1 and m2 attached by a rigid element of length L.
A model of the robot is shown in Figure 1.
Fig. 1: Physical Model of Balancing Robot
The orientation of the robot is determined by θ, the angle of
the robot with respect to vertical. θ must be kept near zero in
order to balance the robot. The system dynamics with respect
to the angle θ are given by equation (1)
¨θ =
(g (m1 + m2) − m1Lcosθ(θ)
2
)sinθ + u cosθ
L(m1 + m2 − m1(cosθ)2)
(1)
In order to measure the states of the system (θ and ˙θ),
both a gyroscope and accelerometer are placed on the robot
to measure ˙θ and ¨θ respectively. The gyroscope measures
˙θ directly, but the accelerometer measures the acceleration
normal to the angle of the robot. This means when θ = 0 that
the accelerometer will be measuring a component of gravity
acceleration. The output from the accelerometer is given in
equation (2), where La is the position of the accelerometer on
the robot relative to the base.
a =
L − La
L
cosθ − m1Lcosθ( ˙θ)2
sinθ
m1 + m2 − m1(cosθ)2
u +
m1 + m2
m1 + m2 − m1(cosθ)2
g sin(θ)
(2)
Both the gyroscope and the accelerometer measurements are
given by yg
k and ya
k respectively in (3a) and (3b).
yg
k = ˙θ(kTs) + vg
k + ng
k (3a)
ya
k = a(kT) + va
k + na
k (3b)
vg
k and va
k are time correlated drift terms and ng
k and na
k are
i.i.d measurement noise.
II. METHODS
Estimation of the system states θ and ˙θ was performed using
both the Extended Kalman Filter and the Unscented Kalman
Filter.
A. Extended Kalman Filter Equations
The traditional Kalman Filter is only applicable for lin-
ear system models. To get around this limitation, the Ex-
tended Kalman Filter (EKF) linearizes the state dynamics,
f(ˆx
(+)
k , uk), in the time update step and the output dynamics,
g(ˆx
(−)
k ), in the measurement update step. The EKF time
update equations are shown in (4) and the measurement update
equations are shown in (5)
Φk =
∂f
∂x
(ˆx
(+)
k , uk) (4a)
ˆx
(−)
k+1 = f(ˆx
(+)
k , uk) (4b)
P
(−)
k+1 = ΦkP
(+)
k ΦT
k + Q (4c)
1

2. Ck =
∂g
∂x
(ˆx
(−)
k ) (5a)
¯Kk = P
(−)
k CT
k CkP
(−)
k CT
k + R
−1
(5b)
ˆx
(+)
k = ˆx
(−)
k + ¯Kk yk − g(ˆx
(−)
k , uk) (5c)
P
(+)
k = (I − ¯KkCk)P
(−)
k (5d)
1) EKF Time Update: In the time update equations, the
first step (4a) is to update the state transition matrix Φk by
taking the derivative of the state jacobian and plugging in the
previous estimate of the mean and the associated input. Then
the prediction of the mean in the next time step is calculated
by plugging in the previous estimate of the mean and the
associated input into the state jacobian (4b). Finally the
covariance is updated in (4c) by multiplying by the transition
matrix Φk and adding in the measurement state noise.
2) EKF Measurement Update: In the measurement update,
the output dynamics are updated in (5a) by plugging the time
updated mean into the derivative of the output jacobian. Then
the Kalman gain is computed in (5b). The mean is updated
in (5c) by adding the previous estimate to the Kalman gain
weighted error between the observed output and the predicted
output. Finally the covariance is updated in (5d) by weighting
the previous covariance with the Kalman gain and output
dynamics.
B. System Model
1) Continuous:
x =


θ
˙θ
Lest

, ˙x = F(x, u) =


˙θ
f
˙L

, y =
˙θ
g
(6)
2) Linearized:
˙x =


0 1 0
∂f
∂θ
∂f
∂ ˙θ
∂f
∂L
0 0 0




θ
˙θ
Lest

 +


0
∂f
∂u
0

 u (7a)
y =
0 1 0
∂g
∂θ
∂g
∂ ˙θ
∂g
∂L


θ
˙θ
Lest

 +
0
∂g
∂u
u (7b)
3) Discretized: When the continuous time system was
converted to a discrete time system in MATLAB, the system
parameters were output as Φ = sys.a, Γ = sys.b, and C =
sys.c. In order to add in the bias terms to the discrete system,
these equations were modified to the equations shown in (8)
Φ =






0 0
sys.a 0 0
0 0
0 0 0 1 0
0 0 0 0 1






5×5
(8a)
Γ =






sys.b
0
0






5×1
(8b)
C =
1 0
sys.c
0 1 2×4
(8c)
The bias of the gyroscope and accelerometer were added to the
system as 4th
and 5th
states. To propagate the bias through the
system in time, an identity matrix was added to the lower left
portion of sys.a in (8a). 2 zeros were added to sys.b in (8b),
since the inputs do not affect the bias. Finally, the biases were
added to the system output in (8c) by appending an identity
matrix to the end of the sys.c matrix. With these modified
discrete system matrices, the complete system including noise
can be modeled using (9).
xk+1 = Φxk + Γuk + wk (9a)
yk = Cxk + Duk + vk (9b)
State: x = [ θ ˙θ Lest bg ba ]T
(9c)
Where, bg and ba are the bias of the gyroscope and bias of
the accelerometer augmented to the state matrix respectively.
4) Initial Conditions and Noise Covariance: Using the
model from the previous section, the only thing needed to
estimate the states of the system are the initial conditions and
the noise covariance (10). The initial state conditions are all
0, with the exception of the length estimate. The initial error
covariance is taken to be very small in all dimensions except in
the dimension corresponding to the unknown length estimate.
Given that the length in dataset 2 is known to be within
10% of 0.38m, it makes sense to set Lvar = (0.1 × 0.38)
2
.
The state and measurement variances (Svar and Mvar) were
both taken to be 0.0001, and the bias variance Bvar was given
to be 0.001 ∗ Tstep
x
(−)
0 =





0
0
Lini
0
0





, P
(−)
0 =





10−5 0 0 0 0
0 10−5 0 0 0
0 0 Lvar 0 0
0 0 0 10−5 0
0 0 0 0 10−5





(10)
2

3. Q =





Svar 0 0 0 0
0 Svar 0 0 0
0 0 0 0 0
0 0 0 Bvar 0
0 0 0 0 Bvar





, R =
Mvar 0
0 Mvar
(11)
C. Unscented Kalman Filter
The Unscented Kalman Filter (UKF) takes a different ap-
proach to estimating the mean and covariance of the condi-
tional distribution of the current state given all previous inputs
and measured outputs. Instead of linearizing the state and
output trajectory equations, the UKF:
1) Takes informed samples around the previous mean esti-
mate (based on the previous error covariance).
2) Puts these samples through the nonlinear trajectory equa-
tions (state trajectory for time update, output trajectory
for measurement update) to get a set of sample outputs.
3) Computes the new mean and autocovariance estimates
from the first and second moments of the computed set
of sample outputs.
The detailed list of equations is shown below in (12) and (13)
1) UKF Time Update:
xk+1 = f(xk, uk) + wk (12a)
P
(+)
k = MMT
(12b)
ei
= [0 0 · · · 1 · · · 0]T
i = 1, · · · , n (12c)
x(i)
= ˆx
(+)
k + ˜x(i)
(12d)
˜x(i)
=
√
nMei
i ≤ n
−
√
nMei−n
i > n
(12e)
x
(i)
k+1 = f(x(i), uk) + wk (12f)
ˆx
(−)
k+1 =
1
2n
2n
i=1
x
(i)
k+1 (12g)
P
(−)
k+1 =
1
2n
2n
i=1
(x
(i)
k+1 − ˆx
(−)
k+1)(x
(i)
k+1 − ˆx
(−)
k+1)T
+ Q
(12h)
Where,
i.) M is obtained by taking the square root decomposition
of P
(+)
k (12b).
ii.) ei is a zero vector with a single one element located at
the ith
element (12c), n is the number of states.
iii.) ˜x(i)
is a group of the offsets from the previous mean
(12e)
2) UKF MEasurement Update:
yk = g(xk, uk) + vk (13a)
P
(−)
k = MMT
(13b)
ei
= [0 0 · · · 1 · · · 0]T
i = 1, · · · , n (13c)
x
(i)
k = ˆx
(−)
k + ˜x(i)
(13d)
˜x(i)
=
√
nMei
i ≤ n
−
√
nMei−n
i > n
(13e)
y(i)
= g(xk(i), uk) (13f)
¯y =
1
2n
2n
i=1
y(i)
(13g)
Pxy =
1
2n
2n
i=1
(x
(i)
k − ˆx
(−)
k )(y(i)
− ¯y)T
(13h)
Py =
1
2n
2n
i=1
(y(i)
− ¯y)(y(i)
− ¯y)T
+ R (13i)
K = PxyPy
−1
(13j)
x
(+)
k = ˆx
(−)
k + K(y − ¯y) (13k)
P
(+)
k = P
(−)
k − KPxy
T
(13l)
Where,
i.) K is the Kalman gain (13j).
ii.) Pxy is the cross co-variance between the estimated state
and output (13h).
D. Testing and Assessment
Both the EKF and the UKF were implemented in
MATALB/Simulink and tested on 3 distinct sets of data.
• The first dataset was generated directly from the model
with known bias, state noise, measurement noise, and
length values.
• The next dataset was generated using a known bias,
measurement nose, and length value, but an unknown
state noise.
• The last dataset was generated using a known bias and
measurement nose, but an unknown state noise and length
value.
In order to asses the success of the EKF and UKF on these
datasets, it was seen first whether the correct values were
obtained for the known dataset. Once proved to be functional,
the algorithms were tested on the datasets with unknown
parameters. The only metric for judging success on these
datasets was the tracking of ˙θ in and the estimation of L.
The bias estimates should also be similar between the two
datasets.
3

4. III. RESULTS
A. Self Generated dataset
1) EKF - Self Generated Data Results:
Fig. 2: State 1, θ Fig. 3: State 2, ˙θ
Fig. 4: Gyroscope Bias Fig. 5: Accelerometer Bias
2) UKF - Self Generated Data Results:
Fig. 6: State 1, θ Fig. 7: State 2, ˙θ
Fig. 8: Gyroscope Bias Fig. 9: Accelerometer Bias
3) EKF vs. UKF: Estimating L for Self Generated Data:
Fig. 10: EKF: L Fig. 11: UKF: L
The Simulink model in Figure 12 was used to create a
dataset with all parameters known. The model in Figure 13
shows the simulink model used to simulate the system model
for one time sample (0.01 seconds) using initial conditions for
all the states to get the states after one time sample.
Fig. 12: Simulink model used to Generate Data
Fig. 13: Simulink model - One step model
The state estimations for both the EKF and UKF (Figures
2, 3, 6, and 7) both track the actual state very closely. The bias
estimations (Figures 4, 5, 8, and 9) are relatively accurate, but
have severe discontinuities. These peaks are most likely caused
at points where the measurements are relatively constant, so
all error becomes attributed to the bias terms. The estimation
of length in both the EKF and UKF settle out to the correct
values within one second. Note that length values of 0.38m
and 0.41m were used for generating the data used to test the
EKF and UKF respectively.
4

5. B. dataset 1
1) EKF - Dataset 1 Results:
Fig. 14: State 1, θ Fig. 15: State 2, ˙θ
Fig. 16: Gyroscope Bias Fig. 17: Accelerometer Bias
2) UKF - Dataset 1 Results:
Fig. 18: State 1, θ Fig. 19: State 2, ˙θ
Fig. 20: Gyroscope Bias Fig. 21: Accelerometer Bias
3) EKF vs. UKF: Estimating L for Dataset 1:
Fig. 22: EKF: L Fig. 23: UKF: L
The state estimations for both the EKF and UKF (Figures
14, 15, 18, and 19) both track the states relatively smoothly.
The estimation of ˙θ (Blue, Figures 15 and 19) compared
to the gyroscope output (Red, Figures 15 and 19) offers
some insight into how well each estimator is tracking the
states. In both cases the estimation begins to diverge from
the gyroscope output over time. However, we know that the
gyroscope is subject to time correlated drift, so we would
expect this kind of behavior. If the estimated gyroscope
bias (Figures 16, and 20) are added to the estimated ˙θ,
then the gyroscope measurement and the estimated ˙θ are
approximately equal. Thus both the EKF and the UKF are
tracking the states and biases well in dataset 1.
The length estimates (Figures 22 and 23) do not fare so
well. After less than 1 second both of the estimates converge
to approximately 0.2m with a low error covariance of 10−9
.
However, the expected result for dataset 1 (which has a known
length) was 0.38m. One possible reason for the difference in
the converged value and the actual value could have to do
with the nature of the system and the control input. Since the
control input is stable (evidenced by the small values of θ
in Figure 14 and 18), which is trying to balance the Segway
in an upright position, there may be multiple distinct lengths
that could result in the same states of angular position and
velocity. Thus, while 0.2m is not the true length of the robot
used to generate the data, due to our initial guess and initial
length variance, it produces very similar state trajectories and
becomes the estimate for the length. The self generated data
demonstrates an unstable input that causes the Segway to
rotate uncontrollably. Using the gyroscope and accelerometer
measurements, the exact length for which such a motion
occurs can be estimated easily. In the case of the stable
Segway system in dataset 1, the length estimation is harder
because the deviations in angle are much smaller and could
fit many different lengths. Since the model assumes a large
initial error in the length estimate, the length value is very
likely to converge to a value that is not 0.38m
5

6. C. dataset 2
1) EKF - Dataset 2 Results:
Fig. 24: State 1, θ Fig. 25: State 2, ˙θ
Fig. 26: Gyroscope Bias Fig. 27: Accelerometer Bias
2) UKF - Dataset 2 Results:
Fig. 28: State 1, θ Fig. 29: State 2, ˙θ
Fig. 30: Gyroscope Bias Fig. 31: Accelerometer Bias
3) EKF vs. UKF: Estimating L for Dataset 2:
Fig. 32: EKF: L Fig. 33: UKF: L
The state estimations for both the EKF and UKF (Figures
24, 25, 28, and 29) both track the states relatively smoothly.
The estimation of ˙θ (Blue, Figures 25 and 29) compared to
the gyroscope output (Red, Figures 25 and 29) offers some
insight into how well each estimator is tracking the states. In
both cases the estimation begins to diverge from the gyroscope
output over time. However, we know that the gyroscope is
subject to time correlated drift, so we would expect this kind of
behavior. If the estimated gyroscope bias (Figures 16, and 20)
are added to the estimated ˙θ, then the gyroscope measurement
and the estimated ˙θ are approximately equal. Thus both the
EKF and the UKF are tracking the states and biases well in
dataset 1.
The accelerometer bias for the EKF and UKF (Figures 27
and 31) differ from one another. While they both follow the
same upward trend, there is more oscillation in the EKF bias
estimate. This is most likely due to the differences in the
length estimates (Figures 32 and 33). The EKF length estimate
converges relatively quickly, but the UKF length estimate
is much more unstable. Thus it makes sense that the EKF
acceleration bias has more oscillation, as all this is where all
of the uncertainty is focused in the model rather than the length
estimate. While the EKF length estimate does converge to a
reasonable value, it is not withing the 10% range of 0.38 that
was given for the dataset.
IV. CONCLUSION
Both the EKF and UKF do a very good job of estimating
the states of the balancing robot system. The bias estimates
of gyroscope and accelerometer were very accurate for both
methods.However, the length estimates from the given datasets
did not match the values that were expected. In dataset 1 the
converged lengths were too small, and in dataset 2 one of the
lengths converged to a value outside of the acceptable range
and the other did not converge at all. This project illustrated
the many different difficulties that can arise when trying to
perform state estimation with an uncertain model and in the
presence of state and measurement noise. Coming up with
good estimates for the noise parameters has a huge effect on
the quality of the estimates obtained. The run time for UKF
is 10 times longer than EKF due to the greater number of
Simulink calls per sample.
6