and asω→ 0 Equation (4.20) becomes,
x y ψ ρi θi k|k−1 = x + vxT cos (ψ)− vyT sin (ψ) y + vxT sin (ψ) + vyT cos (ψ) ψ + ωT ρi+ 0 θi+ 0 k−1 . (4.21)
The difference between Equation (4.20) and (4.21) is limited for cases where the time stepT is small given a typical maximum AUV velocity is under 0.5 m/sec. In a typical SLAM missions Equation (4.21) is good enough because most of the discrepancy between the two models can be corrected in the EKF-SLAM update stage. However in the absence of measurements, we stick with the motion model in Equation (4.20) for subsequent prediction to avoid any accumulation of drift.
4.2.2
Sonar Observation Model
An important element of SLAM algorithm is the measurement update which adjusts navig- ation drift based on landmark error. This correction is done based on an observation model. In order for the SLAM algorithm to succeed the observation model needs to be as accurate as possible. In this section we discuss a piecewise-defined observation model for a line segment feature based sonar measurement.
In Section3.2.1a Hessian normal form of a line segment is proposed as the best altern- ative for representing sonar line features. Since angles are periodic in polar representation, a single line can be expressed in multiple forms. For instance, a polar line given by(ρ, θ) can also be represented as(−ρ, θ+π). For much of the SLAM algorithm this does not cause
problem or can be controlled; however, when it comes to computing innovation vector, this can make filters diverge. Hence, as discussed in [3] line segment observation requires two models. For theithmap linem
i = [ρi, θi], the two observation model are given by,
h (ˆxk, mi) = ρi− x cos θi− y sin θi θi− ψ (4.22) and h (ˆxk, mi) = −ρi+ x cos θi+ y sin θi θi− ψ + π . (4.23)
Probably the most important question now is how to choose between Equation (4.22) and Equation (4.23). In [3], this choice is made based on whether the line segment joining current robot position with the map origin intersect with the line mi or not. On the other
hand, in [2] only Equation (4.22) is used and all the problems with the observation model are claimed to be attributed to the increasing non linearity of the model as the robot goes far out from the map origin. Obviously, both the non linearity as well as a wrong observation model play a role for degrading the performance of SLAM navigation, yet it is important to identify which issue is causing a particular error.
A wrong observation model can easily cause filter divergence which is caused by an exaggerated innovation in Equation4.12. However, the innovation vector can also go wrong if the sonar measurement is given in a wrong polar form. For clear understanding of the problem consider the example shown in Figure4.1. A line is observed from different AUV positions{Ak} at time k, based on the position of measurement line zk and map linemi
with respect to the map origin{W } and {Ak} there are four possible scenarios.
Scenario 1: This the most common robot pose configuration which is applicable for most indoor robotics where a vehicle does not go past a boundary line. In such configur- ation the line joining current robot pose with the map origin does not intersect with either the new measurement line zk or the associated map feature mi as shown in
Figure4.1a. In this case the observation model in Equation4.22is sufficient.
Scenario 2: This is less common robot pose configuration which typically happens when the AUV observes a line which is close to it or when the vehicle pass by a convex corner. In such scenario, although the measurement linezklie on one side of both the
(a) (b)
(c) (d)
Figure 4.1: Possible arrangements of measurement line zk and ith map feature mi with
respect to the global frame{W } and robot frame {Ak}. Lines are represented using polar
form[ρ, θ] with respect to either{W } or {Ak} at time k.
robot and the map origin, the associated linemilie between the two (see Figure4.1b).
For such robot pose configuration a correct observation can be obtained by using Equation4.23.
Scenario 3: Opposite to scenario 2 in this scenario the measurement line is between{W } and{Ak}, while the observed map feature does not (see Figure4.1c). For a robot in
such configuration both Equation4.22 and4.23will cause divergence. In stead first the measurement line need to be inverted, i.e., [ρz, θz] becomes [−ρz, θz+ π], then
Equation4.22can be used.
Scenario 4: In the last scenario (see Figure4.1d) both the observed feature and the meas- urement line intersect with the line segment joining{W } and {Ak}. Here the meas-
ation vector computation.
In a typical run the observed feature and measurement pairs alternate among the four pos- sible configuration. Just modifying the observation model does not guarantee a correct in- novation computation for all possible scenarios, even though it is possible to get a successful result for most of the time in a typical trail. For a complete remedy both the observation and the map feature need to be checked and modified accordingly.
The next question will be how to make this checks. For instance in Figure 4.2, to determine if the line l{Ak} = [ρ, θ] intersect with the line segment joining {W } to the
current robot pose {Ak} = [x, y, φ]{W }, consider a point p on the line l which is a new
frame on the line where the parallel component is along the line while the perpendicular component is along the line normalln. From this new co-ordinate frame the robot pose and
the map origin are given byppAandpp0 respectively. The projection of these two vectors
on the line normal givesppA⊥andpp0⊥which mathematically can be expressed as,
pp0⊥ = ρ + x cos (θ− ψ) + y sin (θ − ψ), (4.24)
ppA⊥ = ρ. (4.25)
For the line to intersect the line segment joining the map origin with the robot origin the sign of the two projection has to be different. This check is done both for the measurement and observed line, adding a slight computation cost on the overall SLAM performance which will tackle a possible filter divergence arising from wrong innovation vector as it will be illustrated in Section4.4.