pn, velocity
¯vn and acceleration
¯an.
6.2 Comparing UKF and EKF, with one and two landmarks
This test aims to see how the UKF and EKF performs in various condi-tions. Three different trajectories are used for this test, a constant velocity trajectory, a acceleration trajectory, and a swaying trajectory. The constant velocity trajectory is chosen to see how close the filters will get to the true constant movement. The acceleration trajectory is chosen to see how well the filters responds to a trajectory in constant change. The sway trajectory aims to see how well the filters responds to movement in more than one direction.
It is run two simulations for each trajectory, the first with one landmark the camera focuses on, and the second with two landmarks. The UKF and EKF are run in parallel for each of the simulations, with the same initial conditions and noise values for the process and measurements.
GPS measurements are available for the two first seconds for these tests.
Landmarks
The landmark(s) which the camera focuses on is the same for all the tests, except the numerical test where no landmark are visible to the camera (no measurement updates). The landmark is located at [x = 50, y = 5, z = 20], for the tests with one landmark, and at [x = 50, y = 5, z = 20]and [x=50, y =−5, z=20]for the tests with two landmarks.
Constant Velocity
All movement are along the x-axis. Figure 5 on page 39 show the trajectory plotted through time and space.
• Accelerate to 10 m/s over 5 meters, or 1 second
• Constant velocity of 10 m/s over 140 meters
Acceleration
All movement are along the x-axis. Figure 6 on page 40 show the trajectory plotted through time and space.
• Accelerate to 10 m/s over 50 meters, or 15 second
Swaying
Figure 7 on page 41 show the trajectory plotted through time and space.
All turns are over a radius of 5 meters.
• Accelerate to 10 m/s over 5 meters, or 1 second
• Turn 45 degrees left
• Turn 90 degrees right
• Turn 90 degrees left
• Turn 90 degrees right
• Turn 90 degrees left
• Turn 90 degrees right
• Turn 90 degrees left
• Turn 90 degrees right
• Turn 90 degrees left
• Turn 90 degrees right
• Turn 90 degrees left
• Turn 45 degrees right
• Constant velocity of 10 m/s over 5 meters
0 5 10 15 0
100 200
True Position
p [m]
Time [s]
x y z
0 5 10 15
0 5 10
True Velocity
v [m/s]
Time [s]
x y z
0 5 10 15
0 10 20
True Acceleration
a [m/s2 ]
Time [s]
x y z
(a) Constant velocity trajectory through time. Position
¯
pn, velocity
¯vnand acceleration
¯an.
0
50
100
−50 5 0 10 20
z [m]
y [m]
x [m]
Trajectory Landmark 1 Landmark 2
(b) Constant velocity trajectory
¯
pnthrough space, with landmarks.
Figure 5: Constant velocity trajectory through time and space.
0 5 10 15 0
50 100
True Position
p [m]
Time [s]
x y z
0 5 10 15
0 5 10
True Velocity
v [m/s]
Time [s]
x y z
0 5 10 15
0 1 2
True Acceleration
a [m/s2 ]
Time [s]
x y z
(a) Acceleration trajectory through time. Position
¯
pn, velocity
v¯n and ac-celeration
¯an.
0
20
40
60
0−5 5 0 10 20
z [m]
y [m]
x [m]
Trajectory Landmark 1 Landmark 2
(b) Acceleration trajectory
¯
pnthrough space, with landmarks.
Figure 6: Acceleration trajectory through time and space.
0 5 10 15 20
−200 0 200
True Position
p [m]
Time [s]
x y z
0 5 10 15 20
−10 0 10
True Velocity
v [m/s]
Time [s]
x y z
0 5 10 15 20
−20 0 20
True Acceleration
a [m/s2 ]
Time [s]
x y z
(a) Swaying trajectory through time. Position
¯
pn, velocity
¯vnand accelera-tion¯an.
0
50
100
150
−50 5 0 10 20
z [m]
y [m]
x [m]
Trajectory Landmark 1 Landmark 2
(b) Swaying trajectory
¯
pnthrough space, with landmarks.
Figure 7: Swaying trajectory through time and space.
7 Simulations and Tests
This section shows the results from the tests described in the previous sec-tion.
The first test presented is the numerical accuracy test. The figures shown are the errors plots for the position and velocity estimate in the axis of mo-tion.
For the test where the filters are compared with one and two land-marks, the figures show the mean error plot for the state estimate ˆmNmc(n), where Nmc is the number of Monte Carlo runs and nis the state number.
The figures also show the Monte Carlo calculated standard deviations(n), and the filter estimated standard deviation ˆσ(n), both as positive and neg-ative. The figures are arranged in such a way that the upper two are for the UKF and EKF respectively for the one landmark system, while the bottom two are for the two landmark system.
The error for the position estimates are displayed in meters, the veloc-ity estimates are displayed in meters per second, and the orientation esti-mates are displayed in radians. The figures also have markings for when the landmark(s) comes in and out of sight for the camera, ”Cam in” and
”Cam out” respectively, and when the system stops receiving position up-dates from the GPS, ”GPS cut−o f f”.
7.1 Numerical Accuracy
Figure 8a on the next page shows the errors for the position estimate pnx, and figure 8b on the following page show the errors in the velocity esti-matevnx. As expected, the error shown in the figures suggests that the nu-merical error is inversely proportional to the sampling frequency, where the error is halved when the sampling frequency is doubled. The estima-tions seems tl lag behind with positive acceleration, and race ahead with negative acceleration.
For 50Hz prediction frequency the peak error for the position estimate pnx is ˆe(1) ≈ 0.2m, while it is about halved for 100Hz and halved again for 200Hz, for an acceleration of 20m/s2. The same is seen in the velocity estimatevnx.
0 1 2 3 4 5 6
(a) Error plot for the position estimatepnxfor the numer-ical test
(b) Error plot for the velocity estimatevnxfor the numer-ical test
Figure 8: Error plot for the position pnx and velocity vnx estimate for the numer-ical test, with the sampling frequencies 50Hz, 100Hz and 200Hz, known initial conditions, without noise and no measurement updates