Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include "nonminimalkalmanfilter.h"
00020 #include "nonminimal_state/linearise.h"
00021 #include "../pdf/nonlinearanalyticconditionalgaussian.h"
00022
00023 namespace BFL
00024 {
00025
00026 #define NLinSys NonLinearAnalyticSystemModelGaussianUncertainty
00027 #define NLinMeas NonLinearAnalyticMeasurementModelGaussianUncertainty
00028
00029
00030 NonminimalKalmanFilter::NonminimalKalmanFilter(Gaussian* prior,
00031 unsigned int NrIterations,
00032 vector<NLSysModel*> minimalsysmodels,
00033 vector<NLMeasModel*> minimalmeasmodels,
00034 vector<GiNaC::symbol> makelinear)
00035 : KalmanFilter(prior)
00036 {
00037
00038 Linear = new Linearise(minimalsysmodels, minimalmeasmodels, makelinear);
00039
00040
00041 MinimalState = Linear->NonlinearStateGet();
00042 NonminimalState = Linear->LinearStateGet();
00043
00044
00045 ColumnVector mu_prior(NonminimalState.size()); mu_prior = 0;
00046 SymmetricMatrix sigma_prior(NonminimalState.size()); sigma_prior = 0;
00047 for (unsigned int i=0; i< NonminimalState.size(); i++)
00048 sigma_prior(i+1,i+1) = 333*333;
00049 NonminimalPrior = new Gaussian(mu_prior,sigma_prior);
00050 MinimalPrior = prior;
00051 cout << "nonminimal prior " << *NonminimalPrior << endl;
00052
00053
00054 NonminimalFilter = new IteratedExtendedKalmanFilter(NonminimalPrior, NrIterations);
00055 MinimalFilter = new IteratedExtendedKalmanFilter(MinimalPrior, NrIterations);
00056
00057
00058
00059 vector<GiNaC::symbol> empty_sym(0);
00060 ColumnVector mu_add(NonminimalState.size()); mu_add = 0;
00061 Gaussian additiveNoise(mu_add,NonminimalFilter->PostGet().CovarianceGet());
00062 NonLinearConditionalGaussian pdf(Linear->SubstitutionGet(), empty_sym, MinimalState, additiveNoise);
00063 MinimalMeasModel = new NLinMeas( &pdf );
00064
00065
00066 for (unsigned int i=0; i<NonminimalState.size() ; i++)
00067 cout << NonminimalState[i] << " -> " << Linear->SubstitutionGet()[i] << endl;
00068 }
00069
00070
00071 NonminimalKalmanFilter::~NonminimalKalmanFilter()
00072 {
00073 delete Linear;
00074 delete NonminimalPrior;
00075 delete NonminimalFilter;
00076 delete MinimalFilter;
00077 delete MinimalMeasModel;
00078 }
00079
00080 void
00081 NonminimalKalmanFilter::SysUpdate(SystemModel<ColumnVector>* const sysmodel,
00082 const ColumnVector& u)
00083 {
00084 NonminimalFilter->SysUpdate(Linear->LinearSysModelGet((NLinSys*)sysmodel),u);
00085 }
00086
00087 void
00088 NonminimalKalmanFilter::MeasUpdate(MeasurementModel<ColumnVector,ColumnVector>* const measmodel,
00089 const ColumnVector& z,
00090 const ColumnVector& s)
00091 {
00092 NonminimalFilter->MeasUpdate(Linear->LinearMeasModelGet((NLinMeas*)measmodel),z,s);
00093 }
00094
00095 }
bfl
Author(s): Klaas Gadeyne, Wim Meeussen, Tinne Delaet and many others. See web page for a full contributor list. ROS package maintained by Wim Meeussen.
autogenerated on Mon Feb 11 2019 03:45:12