iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
test-UKF.C
Go to the documentation of this file.
1 /*! @file
2  @author Unknown
3  @copyright GNU Public License (GPL v3)
4  @section License
5  @verbatim
6  // ////////////////////////////////////////////////////////////////////////
7  // The iLab Neuromorphic Robotics Toolkit (NRT) //
8  // Copyright 2010-2012 by the University of Southern California (USC) //
9  // and the iLab at USC. //
10  // //
11  // iLab - University of Southern California //
12  // Hedco Neurociences Building, Room HNB-10 //
13  // Los Angeles, Ca 90089-2520 - USA //
14  // //
15  // See http://ilab.usc.edu for information about this project. //
16  // ////////////////////////////////////////////////////////////////////////
17  // This file is part of The iLab Neuromorphic Robotics Toolkit. //
18  // //
19  // The iLab Neuromorphic Robotics Toolkit is free software: you can //
20  // redistribute it and/or modify it under the terms of the GNU General //
21  // Public License as published by the Free Software Foundation, either //
22  // version 3 of the License, or (at your option) any later version. //
23  // //
24  // The iLab Neuromorphic Robotics Toolkit is distributed in the hope //
25  // that it will be useful, but WITHOUT ANY WARRANTY; without even the //
26  // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR //
27  // PURPOSE. See the GNU General Public License for more details. //
28  // //
29  // You should have received a copy of the GNU General Public License //
30  // along with The iLab Neuromorphic Robotics Toolkit. If not, see //
31  // <http://www.gnu.org/licenses/>. //
32  // ////////////////////////////////////////////////////////////////////////
33  @endverbatim */
34 #include <nrt/Probabilistic/BayesFilters/UKF.H>
35 #include <nrt/Core/Image/Image.H>
36 #include <nrt/Core/Model/Manager.H>
37 #include <nrt/ImageProc/Drawing/Geometry.H>
38 #include <nrt/ImageProc/Drawing/Text.H>
39 #include <nrt/ImageProc/IO/ImageSink/ImageSinks.H>
40 #include <random>
41 
42 // ######################################################################
43 // Setup the world
44 // ######################################################################
45 nrt::Dims<int> worldDims(640,480);
46 double translationalNoise = 0.09;
47 double angularNoise = 0.002;
48 double sensingRangeNoise = 15;
49 double sensingAngleNoise = 0.01;
50 std::random_device rd;
51 std::mt19937 randomgen(rd());
52 
53 // ######################################################################
54 // Define the dimensions of the system
55 // ######################################################################
56 // Our vehicle's state has two field groups:
57 // - A position field group which keeps track of the (x,y) coordinates as well as the orientation of the vehicle
58 // - A velocity field group which keeps track of the translational and angular velocity of the vehicle
59 NRT_CREATE_STATE_FIELD_GROUP(Position, (x)(y)(theta));
60 NRT_CREATE_STATE_FIELD_GROUP(Velocity, (translational)(angular));
61 typedef nrt::StateDefinition<Position, Velocity> VehicleState;
62 
63 // We observe our vehicle with a range and angle
64 NRT_CREATE_STATE_FIELD_GROUP(PolarPosition, (range)(angle));
65 typedef nrt::StateDefinition<PolarPosition> VehicleObservation;
66 
67 // ######################################################################
68 // Define the filter's model
69 // ######################################################################
70 struct FilterModel
71 {
72  struct ControlType { };
73 
74  static nrt::StateVector<VehicleState>
75  predict(nrt::StateVector<VehicleState> const & state, ControlType const & control)
76  {
77  double x = state.access<Position::x>();
78  double y = state.access<Position::y>();
79  double a = state.access<Position::theta>();
80  double dt = state.access<Velocity::translational>();
81  double da = state.access<Velocity::angular>();
82 
83  nrt::StateVector<VehicleState> predictedPosition;
84  predictedPosition.access<Position::x>() = x + cos(a) * dt;
85  predictedPosition.access<Position::y>() = y + sin(a) * dt;
86  predictedPosition.access<Position::theta>() = a + da;
87  predictedPosition.access<Velocity::translational>() = dt;
88  predictedPosition.access<Velocity::angular>() = da;
89 
90 
91  if(predictedPosition.access<Position::theta>() > M_PI)
92  predictedPosition.access<Position::theta>() -= 2*M_PI;
93  if(predictedPosition.access<Position::theta>() < -M_PI)
94  predictedPosition.access<Position::theta>() += 2*M_PI;
95 
96  return predictedPosition;
97  }
98 
99  static nrt::StateVector<VehicleObservation>
100  observe(nrt::StateVector<VehicleState> const & state)
101  {
102  double x = state.access<Position::x>();
103  double y = state.access<Position::y>();
104 
105  nrt::StateVector<VehicleObservation> observation;
106 
107  observation.access<PolarPosition::range>() = sqrt(pow(x - worldDims.width()/2.0, 2) + pow(y - worldDims.height()/2.0, 2));
108  observation.access<PolarPosition::angle>() = atan2(y - worldDims.height() / 2.0, x - worldDims.width() / 2.0);
109 
110  return observation;
111  }
112 };
113 
114 nrt::Image<nrt::PixRGB<nrt::byte>> worldImage;
115 
116 // Some helper functions...
117 nrt::StateVector<VehicleState> moveVehicle(nrt::StateVector<VehicleState> const & currentPosition);
118 nrt::StateVector<VehicleObservation> takeMeasurement(nrt::StateVector<VehicleState> const & vehicleState);
119 void drawVehicle(nrt::Image<nrt::PixRGB<nrt::byte>> & image, nrt::StateVector<VehicleState> const & currentPosition, nrt::PixRGBA<nrt::byte> const & color,
120  int thickness);
121 void drawPrediction(nrt::Image<nrt::PixRGB<nrt::byte>> & image, nrt::GaussianPDF<VehicleState> const & currentPosition, nrt::PixRGBA<nrt::byte> const & color,
122  int thickness);
123 void drawMeasurement(nrt::Image<nrt::PixRGB<nrt::byte>> & image, nrt::StateVector<VehicleObservation> const & observation);
124 
125 int main(int argc, const char **argv)
126 {
127  // Create a new image display
128  nrt::Manager mgr(argc, argv);
129  std::shared_ptr<nrt::ImageSink> display(new nrt::ImageSink);
130  mgr.addSubComponent(display);
131  mgr.launch();
132 
133  worldImage = nrt::Image<nrt::PixRGB<nrt::byte>>(worldDims, nrt::ImageInitPolicy::Zeros);
134 
135  // Setup the initial vehicle state
136  nrt::GaussianPDF<VehicleState> initialState;
137  initialState.mean<Position::x>() = worldDims.width() / 4;
138  initialState.mean<Position::y>() = worldDims.height() / 4;
139  initialState.mean<Position::theta>() = 0;
140  initialState.mean<Velocity::translational>() = 5.0;
141  initialState.mean<Velocity::angular>() = 0.05;
142  initialState.covariance<>() = Eigen::Matrix<double, 5, 5>::Identity() * 0.00001;
143 
144  // Create a state vector to hold the real vehicle state
145  nrt::StateVector<VehicleState> vehicleState(initialState.mean<>());
146 
147  // Setup the movement covariance
148  nrt::GaussianPDF<VehicleState> movementCovariance;
149  movementCovariance.covariance<Velocity::translational>() = pow(translationalNoise, 2.0)*2;
150  movementCovariance.covariance<Velocity::angular>() = pow(angularNoise, 2.0)*2;
151 
152  // Setup the sensor covariance
153  nrt::GaussianPDF<VehicleObservation> sensorCovariance;
154  sensorCovariance.covariance<PolarPosition::range>() = pow(sensingRangeNoise, 2.0);
155  sensorCovariance.covariance<PolarPosition::angle>() = pow(sensingAngleNoise, 2.0);
156 
157  // Create the filter
158  nrt::UKF<VehicleState, VehicleObservation, FilterModel>
159  ukfTracker(initialState, movementCovariance.covariance<>(), sensorCovariance.covariance<>());
160 
161  nrt::StateVector<VehicleObservation> observation;
162 
163  nrt::Point2D<int> center(worldDims.width()/2, worldDims.height()/2);
164  nrt::drawCircle(worldImage, nrt::Circle<int>(center, 5), nrt::PixRGB<nrt::byte>(64,64,64), 3);
165  nrt::drawText(worldImage, center, "Radar Station", nrt::PixRGBA<nrt::byte>(255, 255, 255, 128),
166  nrt::PixGray<nrt::byte>(), nrt::SimpleFont::FIXED(8), nrt::TextAnchor::TOP_LEFT, true);
167 
168  for(int timestep = 0; timestep < 500; ++timestep)
169  {
170  nrt::Image<nrt::PixRGB<nrt::byte>> displayImage = worldImage;
171 
172  // Move the actual vehicle
173  vehicleState = moveVehicle(vehicleState);
174 
175  if(timestep % 10 == 0)
176  {
177  // Take a (noisy) measurement of the position of the vehicle
178  observation = takeMeasurement(vehicleState);
179  // Update with a measurement
180  ukfTracker.update(observation);
181  }
182 
183  // Predict
184  ukfTracker.predict();
185 
186  nrt::GaussianPDF<VehicleState> filteredState = ukfTracker.state();
187 
188  NRT_INFO("---------------\nStep " << timestep <<
189  "\nPos [" << vehicleState.access<>().transpose() << "]" <<
190  "\nPred [" << filteredState.mean<>().transpose() << "]");
191 
192  // Draw the world
193  drawVehicle(displayImage, vehicleState, nrt::PixRGBA<nrt::byte>(255, 0, 0, 128), 4);
194  drawPrediction(displayImage, filteredState, nrt::PixRGBA<nrt::byte>(0, 255, 0, 128), 2);
195  drawMeasurement(displayImage, observation);
196  display->out(nrt::GenericImage(displayImage));
197  usleep(100000);
198  }
199 
200  return 0;
201 }
202 
203 // ######################################################################
204 nrt::StateVector<VehicleState> moveVehicle(nrt::StateVector<VehicleState> const & currentPosition)
205 {
206  double x = currentPosition.access<Position::x>();
207  double y = currentPosition.access<Position::y>();
208  double a = currentPosition.access<Position::theta>();
209  double dt = currentPosition.access<Velocity::translational>();
210  double da = currentPosition.access<Velocity::angular>();
211 
212  nrt::StateVector<VehicleState> nextPosition;
213  nextPosition.access<Position::x>() = x + cos(a) * dt;
214  nextPosition.access<Position::y>() = y + sin(a) * dt;
215  nextPosition.access<Position::theta>() = a + da;
216  nextPosition.access<Velocity::translational>() = std::normal_distribution<double>(dt, translationalNoise)(randomgen);
217  nextPosition.access<Velocity::angular>() = std::normal_distribution<double>(da, angularNoise)(randomgen);
218 
219  if(nextPosition.access<Position::theta>() > M_PI)
220  nextPosition.access<Position::theta>() -= 2*M_PI;
221  if(nextPosition.access<Position::theta>() < -M_PI)
222  nextPosition.access<Position::theta>() += 2*M_PI;
223 
224  return nextPosition;
225 }
226 
227 // ######################################################################
228 void drawVehicle(nrt::Image<nrt::PixRGB<nrt::byte>> & image,
229  nrt::StateVector<VehicleState> const & currentPosition,
230  nrt::PixRGBA<nrt::byte> const & color,
231  int thickness)
232 {
233  static nrt::Point2D<int> lastpoint(-1,-1);
234 
235  nrt::Point2D<int> c(currentPosition.access<Position::x>(), currentPosition.access<Position::y>());
236  double angle = currentPosition.access<Position::theta>();
237  nrt::drawCircle(image, nrt::Circle<int>(c, 5), color, thickness);
238  nrt::drawLine(image, nrt::Line<int>(c, c + nrt::Point2D<int>(cos(angle)*10, sin(angle)*10)), color, thickness);
239 
240 
241  if(lastpoint != nrt::Point2D<int>(-1, -1))
242  {
243  nrt::PixRGBA<nrt::byte> darkcolor = color;
244  darkcolor.alpha = 64;
245  nrt::drawLine(worldImage, nrt::Line<int>(lastpoint, c), darkcolor, 1);
246  nrt::drawLine(image, nrt::Line<int>(lastpoint, c), darkcolor, 1);
247  }
248 
249  lastpoint = c;
250 }
251 
252 // ######################################################################
253 void drawPrediction(nrt::Image<nrt::PixRGB<nrt::byte>> & image,
254  nrt::GaussianPDF<VehicleState> const & currentPosition,
255  nrt::PixRGBA<nrt::byte> const & color,
256  int thickness)
257 {
258  static nrt::Point2D<int> lastpoint(-1,-1);
259 
260  nrt::Point2D<int> c(currentPosition.mean<Position::x>(), currentPosition.mean<Position::y>());
261  double angle = currentPosition.mean<Position::theta>();
262  nrt::drawCircle(image, nrt::Circle<int>(c, 5), color, thickness);
263  nrt::drawLine(image, nrt::Line<int>(c, c + nrt::Point2D<int>(cos(angle)*10, sin(angle)*10)), color, thickness);
264 
265  nrt::Polygon<int> ellipse(currentPosition.covarianceEllipse<Position::x, Position::y>());
266  nrt::drawPolygon(image, ellipse, nrt::PixRGBA<nrt::byte>(0, 255, 255, 255), 1);
267 
268  if(lastpoint != nrt::Point2D<int>(-1, -1))
269  {
270  nrt::PixRGBA<nrt::byte> darkcolor = color;
271  darkcolor.alpha = 64;
272  nrt::drawLine(worldImage, nrt::Line<int>(lastpoint, c), darkcolor, 1);
273  nrt::drawLine(image, nrt::Line<int>(lastpoint, c), darkcolor, 1);
274  }
275 
276  lastpoint = c;
277 }
278 // ######################################################################
279 void drawMeasurement(nrt::Image<nrt::PixRGB<nrt::byte>> & image,
280  nrt::StateVector<VehicleObservation> const & observation)
281 {
282  double r = observation.access<PolarPosition::range>();
283  double a = observation.access<PolarPosition::angle>();
284  nrt::Point2D<int> c(worldDims.width()/2.0 + r * cos(a), worldDims.height()/2.0 + r * sin(a));
285  nrt::drawCross(image, c, nrt::PixRGBA<nrt::byte>(255, 255, 255, 64), 30, 1);
286  nrt::drawLine(image, nrt::Line<int>(nrt::Point2D<int>(worldDims.width()/2, worldDims.height()/2), c), nrt::PixRGBA<nrt::byte>(255,255,255,16), 3);
287 
288  nrt::drawCross(worldImage, c, nrt::PixRGBA<nrt::byte>(255, 255, 255, 16), 3, 1);
289 }
290 
291 // ######################################################################
292 nrt::StateVector<VehicleObservation> takeMeasurement(nrt::StateVector<VehicleState> const & vehicleState)
293 {
294  double x = vehicleState.access<Position::x>();
295  double y = vehicleState.access<Position::y>();
296 
297  double randomRangeNoise = std::normal_distribution<double>(0, sensingRangeNoise)(randomgen);
298  double randomAngleNoise = std::normal_distribution<double>(0, sensingAngleNoise)(randomgen);
299 
300  nrt::StateVector<VehicleObservation> observation;
301 
302  observation.access<PolarPosition::range>() =
303  sqrt(pow(x - worldDims.width()/2.0, 2) + pow(y - worldDims.height()/2.0, 2)) + randomRangeNoise;
304 
305  observation.access<PolarPosition::angle>() =
306  atan2(y - worldDims.height() / 2.0, x - worldDims.width() / 2.0) + randomAngleNoise;
307 
308  return observation;
309 }
310