![](bg2.png)
improve the system. In our case the distribution of
noise has been found to be Gaussian in image space
and hence the Kalman filter (Kalman, 1960) can be
applied.
2.1 Iconic Kalman Filter
The Kalman filter is a recursive estimation method
that efficiently estimates and predicts the state of a
discretized, dynamic system from a set of potentially
noisy measurements such that the mean square error
is minimized, giving rise to a statistically optimal es-
timation. Only the previous measurement and the cur-
rent measurement are needed.
The general Kalman filter equations are not di-
rectly applicable to a stream of images without some
additional considerations. If an N × M disparity map
was treated as a single input to one Kalman filter
the measurement covariance matrix R would consist
of (N × M)
2
values, which quickly becomes a pro-
hibitively large number. To make an implementation
feasible, each pixel is assumed independent and con-
sequently assigned a separate Kalman filter. A pixel-
wise Kalman filter is said to be iconic (Matthies et al.,
1989).
The filter is applied to disparities, meaning it is
the disparity value that is estimated. The pixel coordi-
nates are still used when applying the motion model,
but not Kalman filtered. The state vector is therefore
just a scalar, i.e. x
t
= x
t
= d
t
.
2.1.1 State, Measurement and Motion Models
The measurement is the disparity value supplied by
the stereo camera. When the camera moves, the pre-
vious disparity map will in general not represent the
new camera viewpoint. Since depth is available for
each pixel they can be moved individually accord-
ing to how the camera was moved, effectively pre-
dicting the scene based on the previous disparity map
and the ego-motion information of the camera yield-
ing two disparity maps depicting the same scene from
the same view point; a prediction and a measurement.
The ego-motion in this context is linear in world
space but since image formation is non-linear,the mo-
tion becomes non-linear in image space. The standard
Kalman filter assumes a linear system which means
it cannot handle the application of ego-motion when
working in image space. Therefore, the state predic-
tion is handled outside of the Kalman filter by tri-
angulating the input into world space, applying ego-
motion as X
t
= M
R
X
t−1
+ M
T
(where M
T
is transla-
tion and M
R
is rotation), and then back-projecting the
result into image space. The Kalman filter therefore
only handles the state variance.
Support for arbitrary camera poses is accom-
plished by including a transformation of the ego-
motion information to the coordinate system of the
camera, given as ω
T
and ω
R
for translation and rota-
tion, respectively.
The complete motion model will be referred to as
the function Φ(.):
x
t|t−1
= Φ(x
t−1|t−1
, M
T
, M
R
, ω
T
, ω
R
) (1)
2.1.2 Noise Models
The process and measurement variance are both
scalars and assumed to be constant, i.e. Q = q
d
and
R = r
d
. The process variance q
d
reflects the confi-
dence in the predictions and is set to a small number
to filter out moving objects (e.g. 0.001). The mea-
surement variance r
d
represents the noise in the dis-
parity values and is determined by measuring the pre-
cision of the stereo camera. The Kalman gain K is ef-
fectively a weighting between the prediction and the
measurement based on the relative values of q
d
and
r
d
.
The state variance P is also a scalar, hence P = p.
It is initially set to r
d
, and from there on maintained
by the Kalman filter.
2.1.3 Predict Phase and Update Phase
The finished iconic Kalman filter equations become:
Predict
x
t|t−1
= Φ(x
t−1|t−1
, M
T
, M
R
, ω
T
, ω
R
) (2)
P
t|t−1
= P
t−1|t−1
+ q
d
(3)
Update
K
t
= P
t|t−1
P
t|t−1
+ r
d
−1
(4)
x
t|t
= x
t|t−1
+ K
t
y
t
− x
t|t−1
(5)
P
t|t
= (1− K
t
)P
t|t−1
(6)
where x is the estimated state, Φ(.) is the mo-
tion model, M
T
/M
R
is the ego-motion as trans-
lation/rotation matrices, ω
T
/ω
R
is the transla-
tion/rotation of ego-motion to camera coordinate sys-
tem, P is the state variance, q
d
is the process variance,
y is the measurement variable, K is the Kalman gain,
and r
d
is the measurement variance.
2.1.4 Update Phase Merging
After the prediction phase has completed and a new
disparity map has been calculated, the update phase
begins. For each pixel, one of three scenarios occur:
FREE SPACE COMPUTATION FROM STOCHASTIC OCCUPANCY GRIDS BASED ON ICONIC KALMAN
FILTERED DISPARITY MAPS
165