| 334 | {} |
| 335 | |
| 336 | void perform_test ( |
| 337 | ) |
| 338 | { |
| 339 | // a basic position + velocity model |
| 340 | matrix<double,2,2> A; |
| 341 | A = 1, 1, |
| 342 | 0, 1; |
| 343 | matrix<double,2,1> B, C; |
| 344 | B = 0, |
| 345 | 1; |
| 346 | |
| 347 | C = 0.02,0.1; // no constant bias |
| 348 | |
| 349 | matrix<double,2,1> Q; |
| 350 | Q = 2, 0; // only care about getting the position right |
| 351 | matrix<double,1,1> R, lower, upper; |
| 352 | R = 1; |
| 353 | |
| 354 | lower = -0.2; |
| 355 | upper = 0.2; |
| 356 | |
| 357 | std::vector<matrix<double,1,1> > controls(30); |
| 358 | std::vector<matrix<double,2,1> > target(30); |
| 359 | for (unsigned long i = 0; i < controls.size(); ++i) |
| 360 | { |
| 361 | controls[i] = 0; |
| 362 | target[i] = 0; |
| 363 | } |
| 364 | |
| 365 | mpc<2,1,30> solver(A,B,C,Q,R,lower,upper); |
| 366 | solver.set_epsilon(0.00000001); |
| 367 | solver.set_max_iterations(10000); |
| 368 | matrix<double,2,1> initial_state; |
| 369 | initial_state = 0; |
| 370 | initial_state(0) = 5; |
| 371 | for (int i = 0; i < 30; ++i) |
| 372 | { |
| 373 | print_spinner(); |
| 374 | matrix<double,1,1> control = solver(initial_state); |
| 375 | |
| 376 | for (unsigned long i = 1; i < controls.size(); ++i) |
| 377 | controls[i-1] = controls[i]; |
| 378 | |
| 379 | // Compute the correct control via SMO and make sure it matches. |
| 380 | solve_linear_mpc(A,B,C,Q,R,lower,upper, target, initial_state, controls); |
| 381 | dlog << LINFO << "ERROR: " << length(control-controls[0]); |
| 382 | DLIB_TEST(length(control-controls[0]) < 1e-7); |
| 383 | |
| 384 | initial_state = A*initial_state + B*control + C; |
| 385 | //cout << control(0) << "\t" << trans(initial_state); |
| 386 | } |
| 387 | |
| 388 | { |
| 389 | // also just generally test our QP solver. |
| 390 | matrix<double,20,20> Q = gaussian_randm(20,20,5); |
| 391 | Q = Q*trans(Q); |
| 392 | |
| 393 | matrix<double,20,1> b = randm(20,1)-0.5; |
nothing calls this directly
no test coverage detected