| 45 | |
| 46 | TEST(StateDS_test){ |
| 47 | RV y("y",1); |
| 48 | RV u("u",1); |
| 49 | RV x("x",2); |
| 50 | |
| 51 | shared_ptr<mlnorm<fsqmat> > IM = new mlnorm<fsqmat>; |
| 52 | IM->set_parameters(mat("1 2 0.5; 0 1 0.3"), zeros(2), fsqmat(1e-10*eye(2))); |
| 53 | IM->set_rv(x); |
| 54 | IM->set_rvc(concat(x.copy_t(-1), u)); |
| 55 | IM->validate(); |
| 56 | |
| 57 | shared_ptr<mlnorm<fsqmat> > OM = new mlnorm<fsqmat>; |
| 58 | OM->set_parameters(mat("1 0"), zeros(1), fsqmat(1e-10*eye(1))); |
| 59 | OM->set_rv(y); |
| 60 | OM->set_rvc(x); |
| 61 | OM->validate(); |
| 62 | |
| 63 | StateDS sds; |
| 64 | sds.set_parameters(IM,OM); |
| 65 | sds.validate(); |
| 66 | |
| 67 | for (int t=1; t<10;t++){ |
| 68 | sds.write(vec_1(double(t))); |
| 69 | sds.step(); |
| 70 | } |
| 71 | |
| 72 | vec dt; sds.getdata(dt); |
| 73 | CHECK_CLOSE(vec("94.5, 94.5, 13.5, 9"), dt, 1e-2); |
| 74 | } |