M-File Help: RangeBearingSensor | View code for RangeBearingSensor |
Range and bearing sensor class
A concrete subclass of the Sensor class that implements a range and bearing angle sensor that provides robot-centric measurements of landmark points in the world. To enable this it holds a references to a map of the world (LandmarkMap object) and a robot (Vehicle subclass object) that moves in SE(2).
The sensor observes landmarks within its angular field of view between the minimum and maximum range.
reading | range/bearing observation of random landmark |
h | range/bearing observation of specific landmark |
Hx | Jacobian matrix with respect to vehicle pose dh/dx |
Hp | Jacobian matrix with respect to landmark position dh/dp |
Hw | Jacobian matrix with respect to noise dh/dw |
g | feature position given vehicle pose and observation |
Gx | Jacobian matrix with respect to vehicle pose dg/dx |
Gz | Jacobian matrix with respect to observation dg/dz |
W | measurement covariance matrix (2x2) |
interval | valid measurements returned every interval'th call to reading() |
landmarklog time history of observed landmarks
Robotics, Vision & Control, Chap 6, Peter Corke, Springer 2011
Sensor, Vehicle, LandmarkMap, EKF
Range and bearing sensor constructor
s = RangeBearingSensor(vehicle, map, options) is an object representing a range and bearing angle sensor mounted on the Vehicle subclass object vehicle and observing an environment of known landmarks represented by the LandmarkMap object map. The sensor covariance is W (2x2) representing range and bearing covariance.
The sensor has specified angular field of view and minimum and maximum range.
'covar', W | covariance matrix (2x2) |
'range', xmax | maximum range of sensor |
'range', [xmin xmax] | minimum and maximum range of sensor |
'angle', TH | angular field of view, from -TH to +TH |
'angle', [THMIN THMAX] | detection for angles betwen THMIN and THMAX |
'skip', K | return a valid reading on every K'th call |
'fail', [TMIN TMAX] | sensor simulates failure between timesteps TMIN and TMAX |
'animate' | animate sensor readings |
options for Sensor constructor
RangeBearingSensor.reading, Sensor.Sensor, Vehicle, LandmarkMap, EKF
Compute landmark location
p = S.g(x, z) is the world coordinate (2x1) of a feature given the observation z (1x2) from a vehicle state with x (3x1).
RangeBearingSensor.Gx, RangeBearingSensor.Gz
Jacobian dg/dx
J = S.Gx(x, z) is the Jacobian dg/dx (2x3) at the vehicle state x (3x1) for sensor observation z (2x1).
Jacobian dg/dz
J = S.Gz(x, z) is the Jacobian dg/dz (2x2) at the vehicle state x (3x1) for sensor observation z (2x1).
Landmark range and bearing
z = S.h(x, k) is a sensor observation (1x2), range and bearing, from vehicle at pose x (1x3) to the k'th landmark.
z = S.h(x, p) as above but compute range and bearing to a landmark at coordinate p.
z = s.h(x) as above but computes range and bearing to all map features. z has one row per landmark.
RangeBearingSensor.reading, RangeBearingSensor.Hx, RangeBearingSensor.Hw, RangeBearingSensor.Hp
Jacobian dh/dp
J = S.Hp(x, k) is the Jacobian dh/dp (2x2) at the vehicle state x (3x1) for map landmark k.
J = S.Hp(x, p) as above but for a landmark at coordinate p (1x2).
Jacobian dh/dw
J = S.Hw(x, k) is the Jacobian dh/dw (2x2) at the vehicle state x (3x1) for map landmark k.
Jacobian dh/dx
J = S.Hx(x, k) returns the Jacobian dh/dx (2x3) at the vehicle state x (3x1) for map landmark k.
J = S.Hx(x, p) as above but for a landmark at coordinate p.
Choose landmark and return observation
[z,k] = S.reading() is an observation of a random visible landmark where z=[R,THETA] is the range and bearing with additive Gaussian noise of covariance W (property W). k is the index of the map feature that was observed.
The landmark is chosen randomly from the set of all visible landmarks, those within the angular field of view and range limits. If no valid measurement, ie. no features within range, interval subsampling enabled or simulated failure the return is z=[] and k=0.
© 1990-2014 Peter Corke.