| | 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 | } |