Comparing the UKF and EKF, with one and two landmarks 45

Constant velocity

The resulting figures from this trajectory is shown on the pages 47 to 60, where each figure shows two sets of simulations, the first two with one landmark, and the last two with two landmarks. Each set are estimated with both the UKF and the EKF, where the initial conditions, the input values and the measurements are identical. The figure show the position, velocity and orientation estimates. Figure 5 on page 39 describes the con-stant velocity trajectory.

The table below show where the figures for the tests can be found Nmc F_pred Fcam Figure Page

pⁿ_x 100 50 1 9 47

pⁿ_x 100 50 10 10 48

pⁿ_x 1000 50 10 11 49

pⁿ_y 100 50 10 12 50

pⁿ_y 1000 50 10 13 51

pⁿ_z 100 50 10 14 52

pⁿ_z 1000 50 10 15 53

vⁿ_x 100 50 1 16 54

vⁿ_x 100 50 10 17 55

vⁿ_y 100 50 10 18 56

vⁿ_z 100 50 10 19 57

λ_φ 100 50 10 20 58 λ_θ 100 50 10 21 59 λ_ψ 100 50 10 22 60

where Nmc is the number of Monte Carlo runs, F_pred is the prediction frequency andFcamis the camera update frequency.

Figures 9 on the facing page and 16 on page 54 shows the estimation of the pⁿ_x and vⁿ_x respectively, with a camera update frequency of 1Hz, figures 10 on page 48 and 17 on page 55 shows the same estimates with a camera update frequency of 10Hz.

The results suggests that 1Hz is too low, while 10Hz seems to be suf-ficient to extract enough information about the platforms movement, at this particular velocity. A higher velocity might require a higher update frequency.

The difference between the Monte Carlo simulations for the position estimates

¯

pⁿ with 100 runs Nmc = 100 (figures 10,12 and 14) and Nmc = 1000 (figures 11,13 and 15) seems negligible, which might suggest that 100 Monte Carlo runs are sufficient.

The UKF seems to be able to estimate its own covaraince almost exactly as the covariance calculated by the Monte Carlo simulations. EKF tends to be over optimistic in its covariance estimate, and somewhat less accurate than the UKF, especially in the non-moving directions. As seen especially from the estimates of the position and velocity in the z-direction, shown in the figures 14 on page 52, and 19 on page 57.

Adding a second landmark seems to have a dramatic improvement in the estimation, for both the UKF and EKF. The two landmarks system seems to settle on a fixed standard deviation. It seems that one landmark for the camera is not sufficient.

The differences between the filters seems to get smaller with two land-marks, although the EKF still seems to be over optimistic in its covariance estimate.

0 5 10 15

UKF: 1 Landmark, Monte Carlo x(1).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 1Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(1).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 1Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(1).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 1Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(1).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 1Hz.

ˆ

Figure 9: Monte Carlo error plots for the velocity pⁿ_x [m], for the constant velocity trajectory, with a camera update frequency of 1Hz.

0 5 10 15

UKF: 1 Landmark, Monte Carlo x(1).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(1).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(1).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(1).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 10: Monte Carlo error plots for the velocitypⁿ_x[m], for the constant velocity trajectory, withNmc =100.

0 5 10 15

UKF: 1 Landmark, Monte Carlo x(1).

Pred: 50Hz. MC−runs: 1000. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(1).

Pred: 50Hz. MC−runs: 1000. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(1).

Pred: 50Hz. MC−runs: 1000. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(1).

Pred: 50Hz. MC−runs: 1000. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 11: Monte Carlo error plots for the velocitypⁿ_x[m], for the constant velocity trajectory, withNmc =1000.

0 5 10 15

UKF: 1 Landmark, Monte Carlo x(2).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(2).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(2).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(2).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 12: Monte Carlo error plots for the velocitypⁿ_y[m], for the constant velocity trajectory, withNmc =100.

0 5 10 15

UKF: 1 Landmark, Monte Carlo x(2).

Pred: 50Hz. MC−runs: 1000. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(2).

Pred: 50Hz. MC−runs: 1000. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(2).

Pred: 50Hz. MC−runs: 1000. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(2).

Pred: 50Hz. MC−runs: 1000. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 13: Monte Carlo error plots for the velocitypⁿ_y[m], for the constant velocity trajectory, withNmc =1000.

0 5 10 15

UKF: 1 Landmark, Monte Carlo x(3).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(3).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(3).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(3).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 14: Monte Carlo error plots for the velocitypⁿ_z [m], for the constant velocity trajectory, withNmc =100.

0 5 10 15

UKF: 1 Landmark, Monte Carlo x(3).

Pred: 50Hz. MC−runs: 1000. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(3).

Pred: 50Hz. MC−runs: 1000. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(3).

Pred: 50Hz. MC−runs: 1000. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(3).

Pred: 50Hz. MC−runs: 1000. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 15: Monte Carlo error plots for the velocitypⁿ_z [m], for the constant velocity trajectory, withNmc =1000.

0 5 10 15

UKF: 1 Landmark, Monte Carlo x(4).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 1Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(4).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 1Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(4).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 1Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(4).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 1Hz.

ˆ

Figure 16: Monte Carlo error plots for the velocityvⁿ_x [m/s], for the constant ve-locity trajectory, with a camera update frequency of 1Hz.

0 5 10 15

UKF: 1 Landmark, Monte Carlo x(4).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(4).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(4).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(4).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 17: Monte Carlo error plots for the velocityvⁿ_x [m/s], for the constant ve-locity trajectory, with a camera update frequency of 10Hz.

0 5 10 15

UKF: 1 Landmark, Monte Carlo x(5).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(5).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(5).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(5).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 18: Monte Carlo error plots for the velocityvⁿ_y [m/s], for the constant ve-locity trajectory.

0 5 10 15

UKF: 1 Landmark, Monte Carlo x(6).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(6).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(6).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(6).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 19: Monte Carlo error plots for the velocityvⁿ_z [m/s], for the constant ve-locity trajectory.

0 5 10 15

UKF: 1 Landmark, Monte Carlo x(7).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(7).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(7).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(7).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 20: Monte Carlo error plots for the roll orientationλ_φ[rad], for the constant velocity trajectory.

0 5 10 15

UKF: 1 Landmark, Monte Carlo x(8).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(8).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(8).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(8).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 21: Monte Carlo error plots for the pitch orientationλ_θ[rad], for the con-stant velocity trajectory.

0 5 10 15

UKF: 1 Landmark, Monte Carlo x(9).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(9).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(9).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(9).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 22: Monte Carlo error plots for the yaw orientationλ_ψ [rad], for the con-stant velocity trajectory.

Acceleration

The resulting figures from the acceleration trajectory is show on the pages 62 to 70. The figures are built in the same manner as for the constant ve-locity trajectory. Figure 6 on page 40 describes the acceleration trajectory.

The table below show where the figures for the tests can be found Nmc F_pred Fcam Figure Page

pⁿ_x 100 50 10 23 62 pⁿ_y 100 50 10 24 63 pⁿ_z 100 50 10 25 64 vⁿ_x 100 50 10 26 65 vⁿ_y 100 50 10 27 66 vⁿ_z 100 50 10 28 67 λ_φ 100 50 10 29 68 λ_θ 100 50 10 30 69 λ_ψ 100 50 10 31 70

where Nmc is the number of Monte Carlo runs, F_pred is the prediction frequency andFcamis the camera update frequency.

For the acceleration trajectory the difference between the filters seems to be less pronounced, the EKF even seems to be preforming at the level of the UKF, and better for some state estimates.

The effects of adding a second landmark seems to be drastically re-duced for this trajectory compared to the constant velocity trajectory. The only real sign of improvement is in the velocity estimate for the direction of movement, it seems to reduce the diverging of the covariance, for the UKF.

0 5 10 15

UKF: 1 Landmark, Monte Carlo x(1).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(1).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(1).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(1).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 23: Monte Carlo error plots for the velocity pⁿ_x [m], for the acceleration trajectory.

0 5 10 15

UKF: 1 Landmark, Monte Carlo x(2).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(2).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(2).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(2).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 24: Monte Carlo error plots for the velocity pⁿ_y [m], for the acceleration trajectory.

0 5 10 15

UKF: 1 Landmark, Monte Carlo x(3).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(3).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(3).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(3).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 25: Monte Carlo error plots for the velocity pⁿ_z [m], for the acceleration trajectory.

0 5 10 15

UKF: 1 Landmark, Monte Carlo x(4).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(4).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(4).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(4).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 26: Monte Carlo error plots for the velocityvⁿ_x [m/s], for the acceleration trajectory.

0 5 10 15

UKF: 1 Landmark, Monte Carlo x(5).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(5).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(5).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(5).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 27: Monte Carlo error plots for the velocityvⁿ_y [m/s], for the acceleration trajectory.

0 5 10 15

UKF: 1 Landmark, Monte Carlo x(6).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(6).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(6).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(6).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 28: Monte Carlo error plots for the velocityvⁿ_z [m/s], for the acceleration trajectory.

0 5 10 15

UKF: 1 Landmark, Monte Carlo x(7).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(7).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(7).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(7).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 29: Monte Carlo error plots for the roll orientationλ_φ[rad], for the accel-eration trajectory.

0 5 10 15

UKF: 1 Landmark, Monte Carlo x(8).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(8).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(8).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(8).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 30: Monte Carlo error plots for the pitch orientationλ_θ[rad], for the accel-eration trajectory.

0 5 10 15

UKF: 1 Landmark, Monte Carlo x(9).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(9).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(9).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(9).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 31: Monte Carlo error plots for the yaw orientationλ_ψ[rad], for the accel-eration trajectory.

Swaying motion

The resulting figures from this trajectory is show on the pages 73 to 82.

The figures are built in the same manner as for the constant velocity and acceleration trajectory. Figure 7 on page 41 describes the sway trajectory.

The table below show where the figures for the tests can be found Nmc Fpred Fcam Figure Page

pⁿ_x 100 50 10 32 73 pⁿ_x 100 100 10 33 74 pⁿ_y 100 100 10 34 75 pⁿ_z 100 100 10 35 76 vⁿ_x 100 100 10 36 77 vⁿ_y 100 100 10 37 78 vⁿ_z 100 100 10 38 79 λ_φ 100 100 10 39 80 λ_θ 100 100 10 40 81 λ_ψ 100 100 10 41 82

where Nmc is the number of Monte Carlo runs, F_pred is the prediction frequency andFcamis the camera update frequency.

Figure 32 on page 73 show the position pⁿ_x estimates with a prediction frequency of 50Hz, the mean error ˆm diverges quite far from 0. Figure 33 on page 74 show the position pⁿ_x estimates with a prediction frequency of 100Hz, here the mean error ˆm is about halved in contrast with a 50Hz prediction frequency. This suggests that the error mainly comes from nu-merical errors due to accelerations in various directions throughout the trajectory.

The rest of the figures are shown with a 100Hz prediction frequency, to better see the details of the filters performance, due to scaling issues in the figures if the error diverges too far.

The differences between the filters with regards to the covariance seems here to resemble more that of the constant velocity trajectory than the ac-celeration trajectory. The UKF seems to more or less constantly estimate the covariance more accurately then the EKF, and the true covariance also seem somewhat more accurate.

The adding of a second landmark seems though to be more akin to the acceleration trajectory, in that the effects seems to not be that great.

Although some improvement are seen in the divergence of the covariance.

0 5 10 15 20

UKF: 1 Landmark, Monte Carlo x(1).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(1).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(1).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(1).

Pred: 50Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 32: Monte Carlo error plots for the positionpⁿ_x[m], for the sway trajectory, with a prediction frequency of 50Hz.

0 5 10 15 20

UKF: 1 Landmark, Monte Carlo x(1).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(1).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(1).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(1).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 33: Monte Carlo error plots for the positionpⁿ_x[m], for the sway trajectory, with a prediction frequency of 100Hz.

0 5 10 15 20

UKF: 1 Landmark, Monte Carlo x(2).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(2).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(2).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(2).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 34: Monte Carlo error plots for the positionpⁿ_y[m], for the sway trajectory, with a prediction frequency of 100Hz.

0 5 10 15 20

UKF: 1 Landmark, Monte Carlo x(3).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(3).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(3).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(3).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 35: Monte Carlo error plots for the positionpⁿ_z [m], for the sway trajectory, with a prediction frequency of 100Hz.

0 5 10 15 20

UKF: 1 Landmark, Monte Carlo x(4).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(4).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(4).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(4).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 36: Monte Carlo error plots for the velocityvⁿ_x [m/s], for the sway trajec-tory, with a prediction frequency of 100Hz.

0 5 10 15 20

UKF: 1 Landmark, Monte Carlo x(5).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(5).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(5).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(5).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 37: Monte Carlo error plots for the velocityvⁿ_y [m/s], for the sway trajec-tory, with a prediction frequency of 100Hz.

0 5 10 15 20

UKF: 1 Landmark, Monte Carlo x(6).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(6).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(6).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(6).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 38: Monte Carlo error plots for the velocityvⁿ_z [m/s], for the sway trajec-tory, with a prediction frequency of 100Hz.

0 5 10 15 20

UKF: 1 Landmark, Monte Carlo x(7).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(7).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(7).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(7).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 39: Monte Carlo error plots for the roll orientationλ_φ [rad], for the sway trajectory, with a prediction frequency of 100Hz.

0 5 10 15 20

UKF: 1 Landmark, Monte Carlo x(8).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(8).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(8).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(8).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 40: Monte Carlo error plots for the pitch orientationλ_θ[rad], for the sway trajectory, with a prediction frequency of 100Hz.

0 5 10 15 20

UKF: 1 Landmark, Monte Carlo x(9).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 1 Landmark, Monte Carlo x(9).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

UKF: 2 Landmarks, Monte Carlo x(9).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

EKF: 2 Landmarks, Monte Carlo x(9).

Pred: 100Hz. MC−runs: 100. GPS: 10Hz. CAM: 10Hz.

ˆ

Figure 41: Monte Carlo error plots for the yaw orientationλ_ψ[rad], for the sway trajectory, with a prediction frequency of 100Hz.

8 Conclusion

This thesis have investigated the use of the Unscented Kalman Filter and the Extended Kalman Filter to estimate the position, velocity and orienta-tion of a inertial navigaorienta-tion system with a camera for external measure-ment point(s). It looked at the effects of increasing the number of land-marks from one to two. Three main trajectories were tested, constant ve-locity, acceleration over the entire trajectory and a trajectory with turns in the x-y plane.

The Unscented Kalman Filter is shown to generally give a more accu-rate estimate of the states, and especially in the estimation of the covari-ance, than the Extended Kalman Filter. This is expected due to that the UKF propagates both the state and the covariance through the non-linear model. In contrast to the UKF, the EKF only propagates the state through the non-linear model, while the covariance is linearized around the

The Unscented Kalman Filter is shown to generally give a more accu-rate estimate of the states, and especially in the estimation of the covari-ance, than the Extended Kalman Filter. This is expected due to that the UKF propagates both the state and the covariance through the non-linear model. In contrast to the UKF, the EKF only propagates the state through the non-linear model, while the covariance is linearized around the

In document Vision aided INS (sider 57-113)