NonminimalKalmanFilter::NonminimalKalmanFilter(Gaussian* prior, unsigned int NrIterations, vector<NLSysModel*> minimalsysmodels, vector<NLMeasModel*> minimalmeasmodels, vector<GiNaC::symbol> makelinear) : KalmanFilter(prior) { // create linearise Linear = new Linearise(minimalsysmodels, minimalmeasmodels, makelinear); // create STATE MinimalState = Linear->NonlinearStateGet(); NonminimalState = Linear->LinearStateGet(); // create PRIOR ColumnVector mu_prior(NonminimalState.size()); mu_prior = 0; SymmetricMatrix sigma_prior(NonminimalState.size()); sigma_prior = 0; for (unsigned int i=0; i< NonminimalState.size(); i++) sigma_prior(i+1,i+1) = 333*333; NonminimalPrior = new Gaussian(mu_prior,sigma_prior); MinimalPrior = prior; cout << "nonminimal prior " << *NonminimalPrior << endl; // create FILTER NonminimalFilter = new IteratedExtendedKalmanFilter(NonminimalPrior, NrIterations); MinimalFilter = new IteratedExtendedKalmanFilter(MinimalPrior, NrIterations); // create MODEL vector<GiNaC::symbol> empty_sym(0); ColumnVector mu_add(NonminimalState.size()); mu_add = 0; Gaussian additiveNoise(mu_add,NonminimalFilter->PostGet().CovarianceGet()); NonLinearConditionalGaussian pdf(Linear->SubstitutionGet(), empty_sym, MinimalState, additiveNoise); MinimalMeasModel = new NLinMeas( &pdf ); // show substitutions for (unsigned int i=0; i<NonminimalState.size() ; i++) cout << NonminimalState[i] << " -> " << Linear->SubstitutionGet()[i] << endl; }
double HPCRS::logpri()const{ return mu_prior()->logp(data_parent_model()->mu()) + siginv_prior_->logp(data_parent_model()->siginv()); }