| 254 | } |
| 255 | |
| 256 | void test_with_positive_target_error_thresh() |
| 257 | { |
| 258 | // a basic position + velocity model |
| 259 | matrix<double,2,2> A; |
| 260 | A = 1, 1, |
| 261 | 0, 1; |
| 262 | matrix<double,2,1> B, C; |
| 263 | B = 0, |
| 264 | 1; |
| 265 | |
| 266 | C = 0.0,0.0; // no constant bias |
| 267 | |
| 268 | matrix<double,2,1> Q; |
| 269 | Q = 2, 0; // only care about getting the position right |
| 270 | matrix<double,1,1> R, lower, upper; |
| 271 | R = 0.001; |
| 272 | |
| 273 | // We set it up so that the controller that only cares about getting to the target but |
| 274 | // doesn't care about what happens after can just give large controls of -1. It will fly |
| 275 | // past the target once it gets there, but it doesn't care about that. The solver_normal |
| 276 | // however wants to come to rest at the target. So it will have to give a much smaller |
| 277 | // control to get it moving towards the target and then slowly decelerate over time to end |
| 278 | // up stopping. So it will take longer to get to the target, which we test below. |
| 279 | lower = -1.0; |
| 280 | upper = 0.05; |
| 281 | |
| 282 | mpc<2,1,30> solver_normal(A,B,C,Q,R,lower,upper); |
| 283 | solver_normal.set_epsilon(0.00000001); |
| 284 | solver_normal.set_max_iterations(10000); |
| 285 | |
| 286 | mpc<2,1,30> solver_target_thresh = solver_normal; |
| 287 | solver_target_thresh.set_target_error_threshold(1.0); |
| 288 | |
| 289 | matrix<double,2,1> state_normal; |
| 290 | state_normal = 10, 0; |
| 291 | matrix<double,2,1> state_target_thresh = state_normal; |
| 292 | |
| 293 | // Run the control law with and without set_target_error_threshold() called. We should |
| 294 | // observe that the controller using set_target_error_threshold(1) drives us to the target |
| 295 | // state faster, however, only the normal solver results in a rest state at the target |
| 296 | // state. |
| 297 | int time_at_target_normal = -1; |
| 298 | int time_at_target_other = -1; |
| 299 | for (int i = 0; i < 30; ++i) |
| 300 | { |
| 301 | print_spinner(); |
| 302 | const double normal_control = solver_normal(state_normal); |
| 303 | state_normal = A*state_normal + B*normal_control + C; |
| 304 | |
| 305 | const double target_thresh_control = solver_target_thresh(state_target_thresh); |
| 306 | state_target_thresh = A*state_target_thresh + B*target_thresh_control + C; |
| 307 | |
| 308 | const double normal_error = trans(state_normal)*diagm(Q)*state_normal; |
| 309 | const double target_error = trans(state_target_thresh)*diagm(Q)*state_target_thresh; |
| 310 | if (normal_error < 1.0 && time_at_target_normal == -1) |
| 311 | time_at_target_normal = i; |
| 312 | if (target_error < 1.0 && time_at_target_other == -1) |
| 313 | time_at_target_other = i; |
no test coverage detected