Radar SLAM using visual features

RESEARCH Open Access

Radar SLAM using visual featuresJonas Callmer1*, David Törnqvist1, Fredrik Gustafsson1, Henrik Svensson2 and Pelle Carlbom3

Abstract

A vessel navigating in a critical environment such as an archipelago requires very accurate movement estimates.Intentional or unintentional jamming makes GPS unreliable as the only source of information and an additionalindependent supporting navigation system should be used. In this paper, we suggest estimating the vesselmovements using a sequence of radar images from the preexisting body-fixed radar. Island landmarks in the radarscans are tracked between multiple scans using visual features. This provides information not only about theposition of the vessel but also of its course and velocity. We present here a navigation framework that requires noadditional hardware than the already existing naval radar sensor. Experiments show that visual radar features canbe used to accurately estimate the vessel trajectory over an extensive data set.

I. IntroductionIn autonomous robotics, there is a need to accuratelyestimate the movements of a vehicle. A simple move-ment sensor like a wheel encoder on a ground robot ora pit log on a vessel will under ideal circumstances pro-vide quite accurate movement measurements. Unfortu-nately, they are sensitive to disturbances. For example,wheel slip due to a wet surface will be interpreted incor-rectly by a wheel encoder, and strong currents will notbe correctly registered by the pit log why a position esti-mate based solely on these sensors will drift off. Inapplications like autonomous robotics, the movementaccuracy needs to be high why other redundant move-ment measurement methods are required.A common approach is to study the surroundings and

see how they change over time. By relating the measure-ments of the environment k seconds ago to the presentones, a measurement of the vehicle translation and rota-tion during this time interval can be obtained. A systemlike this complements the movement sensor andenhances the positioning accuracy.Most outdoor navigation systems such as surface ves-

sels use global navigation satellite systems (GNSS) suchas the Global Positioning System (GPS) to measure theirposition. These signals are weak making them very vul-nerable to intentional or unintentional jamming [1-3]. Asupporting positioning system that is redundant of thesatellite signals is therefore necessary. By estimating the

vessel movements using the surroundings, a mean ofmeasuring the reliability of the GPS system is provided.The movement estimates can also be used during a GPSoutage providing accurate position and movement esti-mates over a limited period of time. This support sys-tem could aid the crew in critical situations during aGPS outage, avoiding costly PR disasters such as run-ning aground.For land-based vehicles or surface vessels, three main

sensor types exist that can measure the environment:cameras, laser range sensors and radar sensors. Camerasare very rich in information and have a long reach butare sensitive to light and weather conditions. Laserrange sensors provide robust and accurate range mea-surements but also they are very sensitive to weatherconditions. The radar signal is usually the least informa-tive signal of the three and is also quite sensitive towhat the signals reflect against. On the other hand, theradar sensor works pretty much equally well in allweather conditions.In this paper, radar scan matching to estimate relative

movements is studied. The idea is to use the radar as animagery sensor and apply computer vision algorithms todetect landmarks of opportunity. Landmarks that occurduring consecuting radar scans are then used for visualodometry, that gives speed, relative position and relativecourse. The main motivation for using visual features tomatch radar scans instead of trying to align the radarscans is that visual features are easily matched despitelarge translational and rotational differences, which ismore difficult using other scan matching techniques.

* Correspondence: [email protected] of Automatic Control, Linköping University Linköping, SwedenFull list of author information is available at the end of the article

Callmer et al. EURASIP Journal on Advances in Signal Processing 2011, 2011:71http://asp.eurasipjournals.com/content/2011/1/71

© 2011 Callmer et al; licensee Springer. This is an Open Access article distributed under the terms of the Creative Commons AttributionLicense (http://creativecommons.org/licenses/by/2.0), which permits unrestricted use, distribution, and reproduction in any medium,provided the original work is properly cited.

mailto:[email protected]

http://creativecommons.org/licenses/by/2.0

The landmarks can optionally be saved in a map formatthat can be used to recognize areas that have been vis-ited before. That is, a by-product of the robust naviga-tion solution is a mapping and exploration system.Our application example is based on a military patrol

boat, Figure 1, that often maneuvers close to the shorein high speeds, at night, without visual aid in situationswhere GPS jamming or spoofing cannot be excluded. Asthe results will show, we are able to navigate in a com-plex archipelago using only the radar and get a mapthat is very close to ground truth.To provide a complete backup system for GPS, global

reference measurements are necessary to eliminate thelongterm drift. The surface navigation system in [4,5],assumed that an accurate sea chart is available. The ideawas to apply map matching between the radar imageand the sea chart, and the particle filter was used forthis mapping. Unfortunately, commercial sea charts stillcontain rather large absolute errors of the shore, see[1,2], which makes them less useful in blind navigationwith critical maneuvers without visual feedback.The radar used in these experiments measures the dis-

tances to land areas using 1,024 samples in each direc-tion, and a full revolution is comprised of roughly 2,000directions. Each scan has a radius of about 5 km givinga range resolution of roughly 5 m. These measurementsare used to create a radar image by translating the rangeand bearing measurements into Cartesian coordinates.An example of the resulting image is shown in Figure 2.The radar image gives a birds eye view of the sur-

rounding islands and by tracking these islands, informa-tion about how the vessel is moving is obtained. We usethe Scale-Invariant Feature Transform (SIFT) [6] toextract trackable features from the radar image whichare subsequently matched with features from laterscans. These features are shown to be distinct and stableenough to be used for island tracking. Other featuredetectors like Speeded Up Robust Features (SURF) [7]

could equally well have been used. When these featuresare tracked using a filter, estimates of the vessel move-ments are obtained that over time give an accurate tra-jectory estimate.The outline is as follows; Section II gives a overview of

the related work followed by a theoretical filtering fra-mework in Section III. In Section IV, the performanceof SIFT is evaluated on radar images, and the trajectoryestimation performance on experimental data is given inSection V. The paper then ends in Section VI with con-clusions and suggested future work.

II. Background and relation to slamThe approach in this contribution is known as theSimultaneous Localization And Mapping (SLAM) pro-blem. Today, SLAM is a fairly well-studied problemwith solutions that are reaching some level of maturity[8,9]. SLAM has been performed in a wide variety ofenvironments such as indoors [10], in urban [11-14] andrural areas [13,15], underwater [16,17] and in the air[18] and the platform is usually equipped with a multi-tude of sensors such as lasers, cameras, inertial measure-ment units, wheel encoders, etc. In this work, we willuse only the radar sensor of a naval vessel to performSLAM in a maritime environment. The data used wererecorded in the Stockholm archipelago by Saab BoforsDynamics [19].Radars have been used for a long time to estimate

movements, for example in the early experiments byClark and Durrant-Whyte [20]. Radar reflecting beaconsin known positions were tracked using a millimeter

Figure 1 The high-speed patrol boat type used for the dataacquisition. Note the backwash created by the jet propulsionsystem. Courtesy of Dockstavarvet AB.

Figure 2 Typical radar image showing the islands surroundingthe vessel. The radar disturbances close to the vessel are causedby the vessel and the waves. Behind the vessel (lower part of theimage), the striped-shaped disturbances are the result ofbackwashes reflecting the radar pulses.


Page 2 of 11

radar, and this was shown to improve the movementestimates. Shortly after, Clark and Dissanayake [21]extended the work by tracking natural features insteadof beacons.Thereafter, laser range sensors became more popular

since they are more reliable, giving a range measure-ment in all directions. The problem of estimating thevehicle movements became a problem of range scanalignment. This was studied among others in [22-25].The advantages of the radar, such as its ability to func-

tion in all weather conditions, have though resulted in itmaking a comeback. Lately, microwave radars have beenused in SLAM experiments but now using a landmark freeapproach. In [13], SLAM was performed in both urbanand rural areas by aligning the latest radar scan with theradar map using 3D correlations to estimate the relativemovements of the vehicle. The radar map was constructedby consecutively adding the latest aligned radar scan tothe previous scans. Checchin et al. [14] performed SLAMin an urban scenario by estimating the rotation and trans-lation of the robot over a sequence of scans using theFourier-Mellin Transform. It can match images that aretranslated, rotated and scaled and can therefore be used toalign radar scans [26]. Chandran and Newman [27] jointlyestimated the radar map and the vehicle trajectory bymaximizing the quality of the map as a function of amotion parametrization.Millimeter wave radars have also become more com-

monplace in some segments of the automotive industry,and the number of applications for them are growing.For example, the road curvature has been estimatedusing the radar reflections that will be used in future sys-tems in collision warning and collision avoidance [28,29].The problem of radar alignment is also present in

meteorology where space radar and ground radar obser-vations are aligned to get a more complete picture ofthe weather in [30]. The scans are aligned by dividingthem into smaller volumes that are matched by theirrespective precipitation intensities.Visual features like SIFT or SURF have been used in

camera-based SLAM many times before. Sometimes, thefeatures were used to estimate relative movements[16,31,32], and other times, they were used to detectloop closures [10,17,33,34].The combination of radar and SIFT has previously

been explored by Li et al. in [35], where Synthetic Aper-ture Radar measurements were coregistered usingmatched SIFT features. Radar scan matching using SIFTwas also suggested in the short papers [36,37]. A systemwith parallel stationary ground radars is discussed andSIFT feature matching is suggested as a way to estimatethe constant overlaps between the scans. No radar scansever seem to be matched in those papers though. Tothe best of the authors knowledge, this is the first time

visual features have been used to estimate the rotationaland translational differences between radar images.

III. Theoretical frameworkAll vessel movements are estimated relative a globalposition. The positions of the tracked landmarks are notmeasured globally but relative to the vessel. Therefore,two coordinate systems are used, one global for posi-tioning the vessel and all the landmarks and one localrelating the measured feature positions to the vessel.Figure 3 shows the local and global coordinate systems,the vessel and a landmark m.The variables needed for visual odometry are summar-

ized in Table 1.

A. Detection modelEach radar scan has a radius of about 5 km with a rangeresolution of 5 meters, and the antenna revolution takesabout 1.5 s.If a landmark is detected at time t, the radar provides

a range, rt, and bearing, θt, measurement to the islandlandmark i as

yit =

(rit

θ it

)+ ei

t (1)

where eit is independent Gaussian noise. These echos

are transformed into a radar image using polar to rec-tangular coordinates conversion, and the result is shownin Figure 2. Figures 1 and 2 also show that the forwardand sideways facing parts of the scans are the most use-ful ones for feature tracking. This is due to the

X

Y

y m

a

b

x

Figure 3 The global (X, Y ) and the local boat fixed (x, y)coordinate systems, the course ψ and the crab angle j givingdifference between the course and velocity vector. The vesseland an island landmark m are also depicted as is the measuredbearing θ and range r of the landmark relative to the vessel.


Page 3 of 11

significant backwash created by the jet propulsion sys-tem of the vessel, which is observed along the vessel tra-jectory in Figure 1. This backwash disturbs the radarmeasurements by reflecting the radar pulse, resulting inthe stripe-shaped disturbances behind the vessel in Fig-ure 2.SIFT is today a well-established standard method to

extract and match features from one image to featuresextracted from a different image covering the samescene. It is a rotation and affine invariant Harris pointextractor that uses a difference-of-Gaussian function todetermine scale. Harris points are in turn regions in theimage where the gradients of the image are large, mak-ing them prone to stand out also in other images of thesame area. For region description, SIFT uses gradienthistograms in 16 subspaces around the point of interest.In this work, SIFT is used to extract and match fea-

tures from radar images. By tracking the SIFT featuresover a sequence of radar images, information about howthe vessel is moving is obtained.

B. Measurement modelOnce a feature has been matched between two scans,the position of the feature is used as a measurement toupdate the filter.Since the features are matched in Cartesian image

coordinates, the straightforward way would be to usethe pixel coordinates themselves as a measurement.After having first converted the pixel coordinates of thelandmark to coordinates in the local coordinate system,the Cartesian feature coordinates are now related to thevessel states as

yit =

(yi

x,t

yiy,t

)+ ei

t = R(ψt)

(mi

X,t − Xt

miY ,t − Yt

)+(

eiX,t

eiY ,t

)(2)

where yix,t is the measured x-coordinate of feature i in

the local coordinate frame at time t and R(ψt) is the

rotation matrix between the ship orientation and theglobal coordinate system. (X, Y ) and (mi

X, miY) are glo-

bal vessel position and global position of landmark i,respectively.The problem with this approach is that eX,t and eY ,t in

(2) are dependent since they are both mixtures of therange and bearing uncertainties of the radar sensor.These dependencies are also time dependent since themixtures depend on the bearing of the radar sensor.Simply assuming them to be independent will introduceestimation errors.A better approach is to convert the Cartesian land-

mark coordinates back to polar coordinates and usethese as a measurement

yit =

(rit

θ it

)=

⎛⎜⎜⎝√

(miX,t − Xt)

2+ (mi

Y ,t − Yt)2

arctan

(mi

Y ,t − Yt

miX,t − Xt

)− ψt

⎞⎟⎟⎠ +

(ei

r,teiθ ,t

). (3)

This approach results in independent noise parameterser ∼ N(0, σ 2

r ) and eθ ∼ N(0, σ 2θ ), which better reflect the

true range and bearing uncertainties of the range sensor.

C. Motion modelThe system states describing the vessel movements attime instant t are

zt = (Xt Yt vt ψt ωt φt)T (4)

where v is the velocity, ψ is the course, ω is the angu-lar velocity and jt is the crab angle, i.e. the wind andstream induced difference between course and velocityvector (normally small). Due to the size and the speedof the vessel, Figure 1, the crab angle is assumed to bevery small throughout the experiments. The systemstates are more extensively described in Table 1 and arealso shown in Figure 3. We will be using a coordinatedturn model, though there are many possible motionmodels available.When landmarks at unknown positions are tracked to

estimate the movements of the vessel, these should bekept in the state vector. If the same landmarks aretracked over a sequence of radar scans, a better estimateof the vessel movement is acquired than if they aretracked between just two.The system states are therefore expanded to also

include all landmarks within the field of view to create avisual odometry framework. The new state vectorbecomes

zt = (Xt Yt vt ψt ωt φt mkX,t mk

Y ,t ... mlY ,t)

T . (5)

Only the l - k + 1 latest landmarks are within the fieldof view why only these are kept in the state vector. Asthe vessel travels on, the landmarks will one by one

Table 1 Summary of notation

Parameter Description

(X, Y) Global position

(x, y) Local coordinate system with x aligned with the stem

v Velocity

ψ Course, defined as the angle between the global X axisand the local x axis, as would be shown by a compass

ω Turn rate, defined as ψ

j Difference between course and velocity vector ("crab”angle), which is mainly due to streams and wind

(miX, mi

Y) Global position of landmark i

ri Range from the strap-down body-fixed radar to landmarki

θi Bearing from the radar to landmark i


Page 4 of 11

leave the field of view why they will be removed fromthe state vector and subsequently replaced by new ones.When all old landmarks are kept in the state vector

even after they have left the field of view, it is a SLAMframework. If an old landmark that left the field of viewlong ago was rediscovered, this would allow for thewhole vessel trajectory to be updated. This is called aloop closure and is one of the key features in SLAM.The SLAM state vector is therefore

zt = (Xt Yt vt ψt ωt φt m1X,t m1

Y ,t . . .)T . (6)

A discretized linearization of the coordinated turnmodel using the SLAM landmark augmentation gives

⎛⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎝

Xt+�t

Yt+�t

vt+�t

ψt+�t

ωt+�t

φt+�t

m1X,t+�t

m1Y ,t+�t

...

⎞⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎠

=

⎛⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎝

Xt + 2vtωt

sin(

ωt�t2

)cos

(ψt + φt + ωt�t

2

)Yt + 2vt

ωtsin

(ωt�t

2

)sin

(ψt + φt + ωt�t

2

)vt + νv,t

ψt + ωt�tωt + νω,t

φt + νφ,t

m1X,t

m1Y ,t...

⎞⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎠(7)

where Δt is the difference in acquisition time betweenthe two latest matched features. νv, νω and νj are inde-pendent Gaussian process noises reflecting the move-ment uncertainties of the vessel.

D. Multi-rate issuesHaving defined a motion model and a measurementmodel, state estimation is usually straightforwardlyimplemented using standard algorithms such as theextended Kalman filter (EKF), the unscented Kalman fil-ter (UKF) or the particle filter (PF), see [38]. These fil-ters iterate between a prediction step based on themotion model and a correction step based on the mea-surement model and the current measurement. Themost natural approach is to stack all landmarks fromone radar revolution into a large measurement vectorand then run the filter with Δt = T = 1.5, where T isthe radar revolution time (1.5 in our application). Thereare, however, two nonstandard problems here.The first problem is that all matched features should

not be used to update the filter at the same time. Sincethe vessel is moving while the radar is revolving, a shiftin the radar image is introduced. A vessel speed of 10m/s will result in a difference in landmark position ofabout 15 m from the beginning to the end of the scan.If the vessel is traveling with a constant velocity andcourse, then all relative changes in feature positionsbetween two scans would be equal, but if the vessel isturning or accelerating, a shift in the relative positionchange is introduced. This results in different features

having different relative movements that will introduceestimation errors if all measurements are used at thesame time. We have found the error caused by thisbatch approach to be quite large. Therefore, the filtershould be updated by each matched featureindependently.If independent course and velocity measurements

were available, they could be used to correct the skew-ness in the radar image. The filter estimates of velocityand course should though not be used for scan correc-tion, since this would create a feedback loop from theestimates to the measurements that can cause filterinstability.Second, if there was one landmark detected in each

scan direction, the filter could be updated with the rate�t = T /N = 1.5

2000 where N is the number of measure-ments per rotation (2,000 in our application). This isnot the case though and we are facing a multi-rate pro-blem with irregularly sampled measurements. The mea-surement model can now conveniently be written as

yt ={

yit if landmark i is detected at time t

NaN otherwise.(8)

Now, any of the filters (EKF, UKF, PF) can be appliedat rate Δt = T/N using (8), with the understanding thata measurement being NaN simply means that the mea-surement update is skipped.

E. Alternative landmark free odometric frameworkThe landmark-based framework derived above suffersfrom one apparent shortcoming: the number of featureswill grow very fast. After only a short time period, thou-sands of potential landmarks will have been found, caus-ing large overhead computations in the implementation.Either a lot of restrictions must be made on which ofthe new landmarks to track, or a different approach isneeded.If the map is not central, an approach based on differ-

ential landmark processing could be taken. Instead oftracking the same feature over a sequence of scans, fea-tures are only matched between two scans to computethe relative movement between the sweeps.1) Relative movement estimation: As described in Sec-

tion III-D, all features from the entire scan should notbe used to estimate the relative movement at the sametime. If the vessel is accelerating or turning, the scanwill be scewed causing estimation errors. The idea istherefore to use subsets of features, measured over ashort time interval, to compute the relative changes incourse Δψt and position ΔXt and ΔYt between twoscans. The scans are simply divided into multiple sliceswhere each segment covers a time interval τt. The


Page 5 of 11

relative movement and course estimates are thereforecalculated multiple times per scan pair.The relative change in position and course can be

described as a relationship between the landmark posi-tions measured in the local coordinate frame at time tand t - T. These landmark positions are related as(

yix,t−T

yiy,t−T

)=(

cos (�ψt) −sin (�ψt)sin (�ψt) cos (�ψt)

) (yi

x,t

yiy,t

)+(

�Xt

�Yt

)(9)

where yix,t is the measured x - coordinate of landmark

i at time instant t in the local coordinate system. yix,t−T

is the measured x - coordinate in the previous scan.If (9) was used to estimate the changes in course and

position between two scans using each segment indepen-dently, quite large course changes could be experienced.Since each scan pair is used multiple times because itdivided into segments, practically the same course andposition change would be calculated over and over again.For example, the change in course registered between thescans will be similar for two adjacent segments. The onlytruly new information in the next segment are thechanges experienced over that specific segment, not thechanges experienced over the rest of the full scan becausethat has already been studied. To avoid calculating thesame course change multiple times, the changes incourse and position can be calculated recursively as

�ψt = �ψt−τ + δψt (10a)

�Xt = �Xt−τ + δXt (10b)

�Yt = �Yt−τ + δYt . (10c)

The change in course is subsequently divided into twoparts: the estimated change in course Δψt-τ using theprevious segment, which is known, and a small changein course δψt experienced during the segment, which isunknown. Even though the vessel used for data acquisi-tion is very maneuverable, δψt can be assumed small.The sine and cosines of (9) can now be rewritten

using (10a) and simplified using the small angle approxi-mation cos (δψt) ≈ 1 and sin (δψt) ≈ δψt

cos (�ψ) = cos (�ψt−τ + δψt)o

= cos (�ψt−τ )︸︷︷︸c�

cos (δψt)︸︷︷︸≈1

− sin (�ψt−τ )︸︷︷︸s�

sin (δψt)︸︷︷︸≈δψt

≈ c� − s�δψt

(11)

sin (�ψ) = sin (�ψt−τ + δψt)

= sin (�ψt−τ )︸︷︷︸s�

cos (δψt)︸︷︷︸≈1

+ cos (�ψt−τ )︸︷︷︸c�

sin (δψt)︸︷︷︸≈δψt

≈ s� + c�δψt

(12)

where cΔ and sΔ are known.Dividing the change in position into two parts as in

(10b) and (10c) does not change the equation system (9)in practice, why ΔXt and ΔYt are left as before.The equation system becomes(

yix,t−T

yiy,t−T

)=(

c� − s�ψtδψt −s� − c� δψt

s� + c�δψt c� − s� δψt

) (yi

x,tyi

y,t

)

+(

�Xt

�Yt

)⇔(

yix,t−T

yiy,t−T

)=((

c� −s�s� c�

)+ δψt

(−s� −c�

c� −s�

)) (yi

x,tyi

y,t

)

+(

�Xt

�Yt

)(13)

which can be rewritten as

(yi

x,t−T − c�yix,t + s�yi

y,t

yiy,t−T − s�yi

x,t − c�yiy,t

)=(−s�yi

x,t − c�yiy,t 1 0

c�yix,t − s�yi

y,t 0 1

) ⎛⎜⎝

δψt

�Xt

�Yt

⎞⎟⎠ (14)

The equation system is now approximately linear andby stacking multiple landmarks in one equation system

⎛⎜⎜⎜⎜⎜⎜⎝

yix,t−T − c�yi

x,t + s�yiy,t

yiy,t−T − s�yi

x,t − c�yiy,t

yjx,t−T − c�yj

x,t + s�yjy,t

yjy,t−T − s�yj

x,t − c�yjy,t

...

⎞⎟⎟⎟⎟⎟⎟⎠ =

⎛⎜⎜⎜⎜⎜⎜⎝

−s�yix,t − c�yi

y,t 1 0c�yi

x,t − s�yiy,t 0 1

−s�yjx,t − c�yj

y,t 1 0c�yj

x,t − s�yjy,t 0 1

...

⎞⎟⎟⎟⎟⎟⎟⎠

⎛⎝ δψt

�Xt

�Yt

⎞⎠ (15)

an overdetermined system is acquired and δψt, ΔXt

and ΔYt can be determined using a least squares solver.This estimated change in position and course can in

turn be used to calculate a velocity and an angular velo-city measurement as

yt =(

vt

ωt

)=

( √(�Xt)

2+(�Yt)2

Tδψtτt

)+(

ev,t

eω,t

). (16)

The measurement noises ev and eω are assumed to beindependent Gaussian noises. This transformation thatprovides direct measurements of speed and coursechange gives what is usually referred to as odometry.Although this approach simplifies the implementation

a lot, it comes with certain drawbacks. First, the land-marks correctly associated between two images are usedonly pairwise, and this is sub-optimal since one losesboth the averaging effects that occur when the samelandmark is detected many times and also the correla-tion structure between landmarks. Second, assuming nocross-correlation between ev and eω is a simplificationsince vt and ωt are based on ΔXt, ΔYt and δψt which arenot calculated independently. Therefore, the measure-ments vt and ωt are actually dependent making the noiseindependence assumption incorrect. And third, in orderto estimate the relative movements, the time interval


Page 6 of 11

used to detect the landmarks must be informativeenough to calculate δψt, ΔXt and ΔYt, but not longenough to allow a significant scan skewedness to appear.This trade-off is vessel specific and must be balanced.By ensuring that the vessel cannot be expected to turnmore than for example 10 degrees during each timeinterval, the small angle approximation holds.2) ESDF: The simplified odometric model above can

still be used for mapping if a trajectory-based filteringalgorithm is used. One such framework is known in theSLAM literature as the Exactly Sparse Delayed-state Fil-ter (ESDF) [39]. It has a state vector that consists ofaugmented vehicle states as

z1:t =(

z1:t−1

zt

), (17)

where the state zt is given in (4) and z1:t-1 are all pre-vious poses. If no loop closures are detected, then theESDF is simply stacking all pose estimates, but once aloop closure is detected and the relative pose betweenthe two time instances is calculated, the ESDF allows forthe whole trajectory to be updated using this newinformation.Once the trajectory has been estimated, all radar scans

can be mapped to world coordinates. By overlaying thescans on the estimated trajectory, a radar map isobtained. Each pixel now describes how many radardetections that have occurred in that coordinate.

IV. SIFT performance on radar imagesSIFT is used to extract visual island features from the radarimages. Figure 4 shows the features that are extracted fromthe upper right quadrant of a radar scan example. Twotypes of features are detected; island features and vessel-related features. The latter originate from radar distur-bances caused by the vessel and the waves and are visiblein the bottom left corner of Figure 4. Unfortunately, thissection of the image cannot just be removed since the ves-sel commonly travels very close to land making island fea-tures in the area sometimes crucial for navigation.The total number of features detected is of course

depending on the number of islands in the area, butalso on where these islands are situated. A large islandclose to the vessel will block a large section of the radarscan, resulting in few features. In these experiments, anaverage of 650 features was extracted per full radar scan.

A. Matching for movement estimationThe SIFT features are matched to estimate the relativemovement of the vessel between multiple consecutivescans. Figure 5a,b shows examples of how well thesefeatures actually match. In Figure 5a, a dense island dis-tribution results in a lot of matches that provide a good

movement estimation. In Figure 5b, there are very fewislands making it difficult to estimate the movementsaccurately.There are two situations that can cause few matches.

One is when there are few islands, and the other is

Figure 4 Radar image with extracted features marked witharrows. This is a typical example of how a radar image from whichSIFT extracts a large number of features may look.

Figure 5 Two pairs of consecutive radar images with detectedmatches marked with lines. a Plenty of islands makes it easy tofind good matching features. b Few islands result in few features tomatch


Page 7 of 11

when a large island is very close to the vessel, blockingthe view of all the other islands. When the vessel passesclose to an island at high speed, the radar scans can dif-fer quite significantly between two revolutions. Thisresults not only in few features to match but also in fea-tures that can be significantly more difficult to matchcausing the relative movement estimates to degrade. Onaverage though, about 100 features are matched in eachfull scan.

B. Loop closure matchingRadar features can also be used to detect loop closuresthat would enable the filter to update the entire vesseltrajectory. The rotation invariance of the SIFT featuresmakes radar scans acquired from different headingsstraightforward to match. Quite a large difference inposition is also manageable due to the range of theradar sensor. This requires of course that no island isblocking or disturbing the view. Figure 6a shows exam-ple locations a, b and c that were used to investigate thematching performance of the visual features.In area a, Figure 6b, and 6b, Figure 6c, the features

are easy to match despite the rather long translationaldifference over open water in b. In both cases, a 180°difference in course is easily overcome by the visual fea-tures. This shows the strength of both the radar sensorand of the visual features. The long range of the sensormakes loop closures over a wide passage of open water

possible. These scans would be used in a full-scaleSLAM experiment to update the trajectory.In area c, Figure 6d, only two features are matched

correctly, and there are also two false positives. If thescans are compared ocularly, it is quite challenging tofind islands that are clearly matching, mostly due toblurring and to blocking islands. It is also noticeablethat the radar reflections from the islands differ due todifferences in radar positions which of course alters theSIFT features. The poor matching result is thereforenatural in this case.

C. Feature preprocessingTwo problems remain that have not been addressed.First is the problem of false feature matches. In Figure6b, a false feature match is clearly visible, and it wouldintroduce estimation errors if not handled properly. Aninitial approach would be to use an algorithm like RAN-SAC[40] to remove matches that are inconsistent withall other matches. One could also use the filtering fra-mework to produce an estimate of the probable featureposition and perform a significance test on the featuresbased on for example Mahalanobis distance. Only thefeatures that pass this test would be allowed to updatethe filter estimates.The other problem is accidental vessel matching. In

heavily trafficked areas like ports, other vessels will bedetected on the radar scans. If a moving vessel is

Figure 6 Examples of loop closure experiments using visual SIFT features. a Example locations used to examine the loop closure detectionperformance. b Multiple matched features from a loop closure in area a in (a). A 180° difference in course is easily handled by the SIFT features.c Location b has both a 180° course difference and a translative change but a couple of features are still matched. d 2 correct and 2 falsematches in area c makes it unsuitable for loop closure updates.


Page 8 of 11

deemed stationary and is used to update the filter, errorswill be introduced. Two approaches could be taken tohandle this problem. Again, a significance test could beused to rule out features from fast moving vessels.Alternatively, a target tracking approach could be used.By tracking the features over multiple scans, the vesselscan be detected and ruled out based on their positioninconsistency compared to the stationary features.Describing such a system is though beyond the scope ofthis paper. The joint SLAM and target tracking problemhas previously been studied in [41].

V. Experimental resultsThe experimental results in this section come from themaster thesis by Henrik Svensson [42]. The implemen-ted framework is the one described in Sections III-E1and III-E2.

A. ResultsThe trajectory used in the SLAM experiment is shownin bold in Figure 7. The track is about 3,000 s long (50min) and covers roughly 32 km. The entire round tripwas unfortunately never used in one single experimentsince it was constituted of multiple data sets.The estimated trajectory with covariance is compared

to the GPS data in Figure 8. The first two quarters ofthe trajectory consist of an island rich environment, seeFigure 6a, resulting in a very good estimate. The thirdquarter covers an area with fewer islands causing theperformance to degrade. This results in an initial misa-lignment of the final quarter that makes the estimatedtrajectory of this segment seem worse than it actually is.Both velocity and course estimates, Figures 9 and 10,

are quite good when compared to GPS data. There isthough a positive bias on the velocity estimate, probablydue to the simplifications mentioned in Section III-E.The course error grows in time since the estimate is the

Figure 7 The whole trajectory where the studied interval ismarked by bold line.

Figure 8 ESDF trajectory estimate with covariance ellipsescompared to GPS measurements.

Figure 9 Velocity estimate compared to GPS measurements. Aslight positive bias is present, probably due to the simplificationsmentioned in Section III-E.

Figure 10 Course estimate compared to GPS measurements.The course estimate is the sum of all estimated changes in course,causing the error to grow in time.


Page 9 of 11

sum of a long sequence of estimated changes in course,see (16), and there are no course measurements avail-able. The final course error is about 30 degrees.

B. Map estimateA radar map of the area was generated by overlaying theradar scans on the estimated trajectory. Figure 11ashows the estimated radar map that should be comparedto the map created using the GPS trajectory, Figure 11b.They are very similar although small errors in the esti-mated trajectory are visible in Figure 11a as blurriness.Some islands appear a bit larger in the estimated mapbecause of this. Overall, the map estimate is good.The estimated map should also be compared to the

satellite photo of the area with the true trajectorymarked in white as shown in Figure 11c. When com-pared, many islands in the estimated map are easilyidentified. This shows that the rather simple approachof using visual features on radar images can providegood mapping results.

VI. ConclusionsWe have presented a new approach to robust navigationfor surface vessels based on radar measurements only.No infrastructure or maps are needed. The basic idea isto treat the radar scans as images and apply the SIFTalgorithm for tracking landmarks of opportunity. Wepresented two related frameworks, one based on theSLAM idea where the trajectory is estimated jointly witha map, and the other one based on odometry. We haveevaluated the SIFT performance and the odometric fra-mework on data from a high-speed patrol boat andobtained a very accurate trajectory and map.An interesting application of this work would be to

apply this method to underwater vessels equipped withsynthetic aperture sonar as the imagery sensor, sincethere are very few low-cost solutions for underwaternavigation.

AcknowledgementsThis work was supported by the Strategic Research Center MOVIII, funded bythe Swedish Foundation for Strategic Research, SSF and CADICS, a Linnaeuscenter funded by the Swedish Research Council. The authors declare thatthey have no competing interests.

Author details1Division of Automatic Control, Linköping University Linköping, Sweden2Nira Dynamics Linköping, Sweden 3Saab Dynamics Linköping, Sweden

Received: 10 December 2010 Accepted: 23 September 2011Published: 23 September 2011

Figure 11 From top to bottom: the estimated radar map of thearea, the true radar map generated using GPS and a satellitephoto of the archipelago. a The estimated map created byoverlaying the radar scans on the estimated trajectory. Theestimated trajectory is marked with a black line. b A radar mapcreated using the GPS measurements of the trajectory. The truetrajectory is marked with a black line. c A satellite photo of the area.The true trajectory is marked with a white line. Many islands in (a)are easily identified in this photo.


Page 10 of 11

References1. J Volpe, Vulnerability assessment of the transportation infrastructure relying

on global positioning system final report. National Transportation SystemsCenter, Tech Rep (2001)

2. GNSS vulnerability and mitigation measures, (rev. 6). European MaritimeRadionavigation Forum, Tech Rep (2001)

3. A Grant, P Williams, N Ward, S Basker, GPS jamming and the impact onmaritime navigation. J Navig. 62(2), 173–187 (2009). doi:10.1017/S0373463308005213

4. M Dalin, S Mahl, Radar Map Matching, Master’s thesis, (Linköping University,Sweden, 2007)

5. R Karlsson, F Gustafsson, Bayesian surface and underwater navigation. IEEETrans Signal Process. 54(11), 4204–4213 (2006)

6. D Lowe, Object recognition from local scale-invariant features, in ICCV ‘99:Proceedings of the International Conference on Computer Vision. 2 (1999)

7. H Bay, T Tuytelaars, L Van Gool, Surf: speeded up robust features, in 9thEuropean Conference on Computer Vision (Graz Austria, May 2006)

8. H Durrant-Whyte, T Bailey, Simultaneous localization and mapping (SLAM):part I. Robot. Autom Mag IEEE. 13(2), 99–110 (2006)

9. T Bailey, H Durrant-Whyte, Simultaneous localization and mapping (SLAM):part II. Robot. Autom Mag IEEE. 13(3), 108–117 (Sept. 2006)

10. P Newman, K Ho, SLAM-loop closing with visually salient features, inProceedings of the IEEE International Conference on Robotics and Automation(ICRA) (2005)

11. MC Bosse, R Zlot, Map matching and data association for large-scale two-dimensional laser scan-based SLAM. Int J Robot Res. 27(6), 667–691 (2008).doi:10.1177/0278364908091366

12. K Granström, J Callmer, F Ramos, J Nieto, Learning to detect loop closurefrom range data, in Proceedings of the IEEE International Conference onRobotics and Automation (ICRA) (2009)

13. R Rouveure, M Monod, P Faure, High resolution mapping of theenvironment with a ground-based radar imager, in Proceedings of theInternational Radar Conference (2009)

14. P Checchin, F Grossier, C Blanc, R Chapuis, L Trassoudaine, Radar scanmatching SLAM using the Fourier-Mellin transform, in IEEE InternationalConference on Field and Service Robotics (2009)

15. F Ramos, J Nieto, H Durrant-Whyte, Recognising and modelling landmarksto close loops in outdoor SLAM, in Proceedings of the IEEE InternationalConference on Robotics and Automation (ICRA) (2007)

16. R Eustice, H Singh, J Leonard, M Walter, R Ballard, Visually navigating theRMS Titanic with SLAM information filters, in Proceedings of Robotics: scienceand Systems (2005)

17. I Mahon, S Williams, O Pizarro, M Johnson-Roberson, Efficient view-basedSLAM using visual loop closures. IEEE Trans Robot. 24(5), 1002–1014 (2008)

18. M Bryson, S Sukkarieh, Bearing-only SLAM for an airborne vehicle, inProceedings of the Australasian Conference on Robotics and Automation(ACRA) (2005)

19. P Carlbom, Radar map matching. (2005) Technical Report20. S Clark, H Durrant-Whyte, Autonomous land vehicle navigation using

millimeter wave radar, in Proceedings of the IEEE International Conference onRobotics and Automation (1998)

21. S Clark, G Dissanayake, Simultaneous localisation and map building usingmillimetre wave radar to extract natural features, in Proceedings of the IEEEInternational Conference on Robotics and Automation (1999)

22. L Feng, E Milios, Robot pose estimation in unknown environments bymatching 2D range scans, in Computer Vision and Pattern Recognition, 1994.Proceedings CVPR ‘94., 1994 IEEE Computer Society Conference on (1994)

23. F Lu, E Milios, Globally consistent range scan alignment for environmentmapping. Auton Robot. 4(4), 333–349 (1997). doi:10.1023/A:1008854305733

24. Y Chen, G Medioni, Object modeling by registration of multiple rangeimages. Image Vis Comput. 10(3), 145–155 (1992). doi:10.1016/0262-8856(92)90066-C

25. F Ramos, D Fox, H Durrant-Whyte, CRF-matching: Conditional random fieldsfor feature based scan matching, in Proceedings of Robotics: science andSystems (2007)

26. Q Chen, M Defrise, F Deconinck, Symmetric phase-only matched filtering offourier-mellin transforms for image registration and recognition. IEEE TransPattern Analysis Mach Intell. 16(12), 1156–1168 (1994). doi:10.1109/34.387491

27. M Chandran, P Newman, Motion estimation from map quality withmillimeter wave radar, in Proceedings of the IEEE/RSJ International Conferenceon Intelligent Robots and Systems (2006)

28. SH Tsang, PS Hall, EG Hoare, NJ Clarke, Advance path measurement forautomotive radar applications. IEEE Trans Intell Transp Syst. 7(3), 273–281(2006). doi:10.1109/TITS.2006.880614

29. C Lundquist, Automotive Sensor Fusion for Situation Awareness, LicentiateThesis No 1422, (Linköping University, Sweden, 2009)

30. SM Bolen, V Chandrasekar, Methodology for aligning and comparingspaceborne radar and ground-based radar observations. J Atmos OceanTechnol. 20, 647–659 (2003). doi:10.1175/1520-0426(2003)202.0.CO;2

31. S Se, D Lowe, J Little, Mobile robot localization and mapping withuncertainty using scale-invariant visual landmarks. Int J Robot Res. 21,735–758 (2002). doi:10.1177/027836402761412467

32. P Jensfelt, D Kragic, J Folkesson, M Bjorkman, A framework for vision basedbearing only 3-D SLAM, in Proceeding IEEE International Conference onRobotics and Automation (ICRA) (2006)

33. M Cummins, P Newman, Probabilistic appearance based navigation andloop closing, in IEEE International Conference on Robotics and Automation(2007)

34. J Callmer, K Granström, J Nieto, F Ramos, Tree of words for visual loopclosure detection in urban slam, in Proceedings of the 2008 AustralasianConference on Robotics and Automation (ACRA) (2008)

35. F Li, G Zhang, J Yan, Coregistration based on SIFT algorithm for syntheticaperture radar interferometry, in Proceedings of ISPRS Congress (2008)

36. M Schikora, B Romba, A framework for multiple radar and multiple 2D/3Dcamera fusion, in 4th German Workshop Sensor Data Fusion: trends,Solutions, Applications (SDF) (2009)

37. H Essen, G Luedtke, P Warok, W Koch, K Wild, M Schikora, Millimeter waveradar network for foreign object detection, in 2nd International Workshopon Cognitive Information Processing (CIP) (2010)

38. F Gustafsson, Statistical Sensor Fusion (Studentlitteratur, Lund, Sweden, 2010)39. R Eustice, H Singh, J Leonard, Exactly sparse delayed-state filters for view-

based SLAM. IEEE Trans Robot. 22(6), 1100–1114 (2006)40. MA Fischler, RC Bolles, Random sample consensus: a paradigm for model

fitting with applications to image analysis and automated cartography.Commun ACM. 24, 381–395 (1981). doi:10.1145/358669.358692

41. CC Wang, C Thorpe, S Thrun, M Hebert, H Durrant-Whyte, Simultaneouslocalization, mapping and moving object tracking. Int J Robot Res. 26(9),889–916 (2007). doi:10.1177/0278364907081229

42. H Svensson, Simultaneous Localization and Mapping in a MarineEnvironment using Radar Images, Master’s thesis, (Linköping University,Sweden, 2009)

doi:10.1186/1687-6180-2011-71Cite this article as: Callmer et al.: Radar SLAM using visual features.EURASIP Journal on Advances in Signal Processing 2011 2011:71.

Submit your manuscript to a journal and benefi t from:

7 Convenient online submission

7 Rigorous peer review

7 Immediate publication on acceptance

7 Open access: articles freely available online

7 High visibility within the fi eld

7 Retaining the copyright to your article

Submit your next manuscript at 7 springeropen.com


Page 11 of 11

http://www.ncbi.nlm.nih.gov/pubmed/21348435?dopt=Abstract



http://www.springeropen.com/

http://www.springeropen.com/

Documents

Radar SLAM using visual features