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());
}
