| | 428 | class UKFCh : public EKFCh{ |
| | 429 | public: |
| | 430 | double kappa; |
| | 431 | |
| | 432 | void bayes ( const vec &yt, const vec &cond = empty_vec ){ |
| | 433 | |
| | 434 | vec &_mu = est._mu(); |
| | 435 | chmat &_P = est._R(); |
| | 436 | chmat &_Ry = fy._R(); |
| | 437 | vec &_yp = fy._mu(); |
| | 438 | |
| | 439 | int dim = dimension(); |
| | 440 | int dim2 = 1+dim+dim; |
| | 441 | |
| | 442 | double npk =dim+kappa;//n+kappa |
| | 443 | mat Xi(dim,dim2); |
| | 444 | vec w=ones(dim2)* 0.5/npk; |
| | 445 | w(0) = (npk-dim)/npk; // mean is special |
| | 446 | |
| | 447 | //step 1. |
| | 448 | int i; |
| | 449 | Xi.set_col(0,_mu); |
| | 450 | |
| | 451 | for ( i=0;i<dim; i++){ |
| | 452 | vec tmp=sqrt(npk)*_P._Ch().get_col(i); |
| | 453 | Xi.set_col(i+1, _mu+tmp); |
| | 454 | Xi.set_col(i+1+dim, _mu-tmp); |
| | 455 | } |
| | 456 | |
| | 457 | // step 2. |
| | 458 | mat Xik(dim,dim2); |
| | 459 | for (i=0; i<dim2; i++){ |
| | 460 | Xik.set_col(i, pfxu->eval(Xi.get_col(i), cond)); |
| | 461 | } |
| | 462 | |
| | 463 | //step 3 |
| | 464 | vec xp=zeros(dim); |
| | 465 | for (i=0;i<dim2;i++){ |
| | 466 | xp += w(i) * Xik.get_col(i); |
| | 467 | } |
| | 468 | |
| | 469 | //step 4 |
| | 470 | mat Xpred_dif(dim2,dim); |
| | 471 | for (i=0;i<dim2;i++){ |
| | 472 | Xpred_dif.set_row(i, sqrt(w(i)) * (Xik.get_col(i) - xp)); |
| | 473 | } |
| | 474 | |
| | 475 | // NEW Sigma points |
| | 476 | mat tmp; |
| | 477 | qr(concat_vertical(Xpred_dif,Q._Ch()), tmp); |
| | 478 | mat Ppred=tmp.get_rows(0,dim-1); |
| | 479 | // New Xi(k+1) |
| | 480 | Xik.set_col(0,xp); |
| | 481 | for ( i=0;i<dim; i++){ |
| | 482 | vec tmp=sqrt(npk)*Ppred.get_col(i); |
| | 483 | Xik.set_col(i+1, _mu+tmp); |
| | 484 | Xik.set_col(i+1+dim, _mu-tmp); |
| | 485 | } |
| | 486 | for (i=0;i<dim2;i++){ |
| | 487 | Xpred_dif.set_row(i, sqrt(w(i)) * (Xik.get_col(i) - xp)); |
| | 488 | } |
| | 489 | |
| | 490 | |
| | 491 | |
| | 492 | //step 5 |
| | 493 | mat Yi(dimy,dim2); |
| | 494 | for (i=0; i<dim2; i++){ |
| | 495 | Yi.set_col(i, phxu->eval(Xik.get_col(i), cond)); |
| | 496 | } |
| | 497 | //step 6 |
| | 498 | _yp.clear(); |
| | 499 | for (i=0;i<dim2;i++){ |
| | 500 | _yp += w(i) * Yi.get_col(i); |
| | 501 | } |
| | 502 | //step 7 |
| | 503 | mat Ypred_dif(dim2,dimy); |
| | 504 | |
| | 505 | for (i=0; i<dim2; i++){ |
| | 506 | Ypred_dif.set_row(i, sqrt(w(i))*(Yi.get_col(i)-_yp)); |
| | 507 | } |
| | 508 | if (!qr(concat_vertical(Ypred_dif,R._Ch()) ,tmp)){ bdm_warning("QR failed");} |
| | 509 | _Ry._Ch() = tmp.get_rows(0,dimy-1); |
| | 510 | |
| | 511 | // step 8 |
| | 512 | mat Pxy=Xpred_dif.T()*Ypred_dif; |
| | 513 | mat iRy=inv(_Ry._Ch()); |
| | 514 | |
| | 515 | //filtering????? -- correction |
| | 516 | mat K=Pxy*iRy*iRy.T(); |
| | 517 | mat K2=Pxy*inv(_Ry.to_mat()); |
| | 518 | |
| | 519 | /////////////// new filtering density |
| | 520 | _mu = xp + K*(yt - _yp); |
| | 521 | // fill the space in Ppred; |
| | 522 | mat Rpred =concat_vertical(Xpred_dif - Ypred_dif*K.T(), _Ry._Ch()*K.T()); |
| | 523 | |
| | 524 | if(!qr(Rpred,tmp)) {bdm_warning("QR failed");} |
| | 525 | _P._Ch()=tmp.get_rows(0,dim-1); |
| | 526 | } |
| | 527 | void from_setting(const Setting &set){ |
| | 528 | EKFCh::from_setting(set); |
| | 529 | kappa = 1.0; |
| | 530 | UI::get(kappa,set,"kappa"); |
| | 531 | } |
| | 532 | }; |
| | 533 | UIREGISTER(UKFCh); |
