Een robot voor de Startel RoboChallenge 2004 - Kunstmatige ...

Sφ1 = Variantie in de hoek van normale verdeling 1.
Sφ2 = Variantie in de hoek van normale verdeling 2.
X1,x2,y1,y2,φ1,φ2 = x/y/hoek positie van normale verdeling 1 of 2.
Figuur 4: Grafische weergave van het Kalman filter.
De normale verdeling 1 is de positie waar de robot denkt dat hij is. Deze bevat naast een
x, y en hoek component varianties voor de x, de y en de hoek. Als de robot rondrijdt
wordt deze normale verdeling aangepast aan de odometrie van de robot. Dat gebeurt
m.b.v. de volgende formules:
Met:
X = rx
+ ∆a
⋅sin(deg2rad(
90 + ϕr
))
Y = ry
+ ∆a
⋅ sin(deg2rad( ϕr
))
= ϕ ϕ
( h)
% 360 ∆ +
∆a = √(∆x 2 +∆y 2 )= afgelegde afstand door de robot
∆x = afgelegde afstand door de robot in de x as
∆y = afgelegde afstand door de robot in de y as
∆h = afgelegde hoek door de robot
φr = hoek van de robot
rx = x-positie van de robot
ry = y-positie van de robot
r
Als de robot achteruit rijdt moet ∆a negatief worden. De variantie van de hoek en van de
x en de y wordt lineair groter met de afstand die de robot heeft afgelegd.
9