FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

ref #39 smoothing is refactored

Browse Source 
KDE smoothing algorithmisch mal geschrieben, jetzt noch testen
This commit is contained in:
toni
2017-12-06 17:37:14 +01:00
parent 95a5c8f34f
commit 74981c6a45
14 changed files with 1303 additions and 127 deletions
Download Patch File Download Diff File Expand all files Collapse all files

213
smc/smoothing/BackwardSimulation.h
View File

@@ -46,8 +46,8 @@ namespace SMC {
/** all smoothed particles T -> 1*/
std::vector<std::vector<Particle<State>>> backwardParticles;
/** container for particles */
std::vector<Particle<State>> smoothedParticles;
/** all estimations calculated */
std::vector<State> estimatedStates;
/** the estimation function to use */
std::unique_ptr<ParticleFilterEstimation<State>> estimation;
@@ -77,26 +77,13 @@ namespace SMC {
BackwardSimulation(int numRealizations) {
this->numRealizations = numRealizations;
backwardParticles.reserve(numRealizations);
smoothedParticles.reserve(numRealizations);
firstFunctionCall = true;
}
/** dtor */
~BackwardSimulation() {
;
}
/** reset **/
void reset(){
this->numRealizations = numRealizations;
backwardParticles.clear();
backwardParticles.reserve(numRealizations);
smoothedParticles.clear();
smoothedParticles.reserve(numRealizations);
firstFunctionCall = true;
estimatedStates.clear();
}
/** access to all backward / smoothed particles */
@@ -140,118 +127,154 @@ namespace SMC {
}
/**
* perform update: transition -> correction -> approximation
* gets the weighted sample set of a standard condensation
* particle filter in REVERSED order!
* @brief update
* @param forwardHistory
* @return
*/
State update(std::vector<Particle<State>> const& forwardParticles) {
State update(ForwardFilterHistory<State, Control, Observation>& forwardHistory, int lag = 666) {
// sanity checks (if enabled)
Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!");
Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!");
//storage for single trajectories / smoothed particles
smoothedParticles.clear();
//init for backward filtering
std::vector<Particle<State>> smoothedParticles;
smoothedParticles.reserve(numRealizations);
firstFunctionCall = true;
// Choose \tilde x_T = x^(i)_T with probability w^(i)_T
// Therefore sample independently from the categorical distribution of weights.
if(firstFunctionCall){
smoothedParticles = sampler->drawTrajectorie(forwardParticles, numRealizations);
firstFunctionCall = false;
backwardParticles.push_back(smoothedParticles);
const State es = estimation->estimate(smoothedParticles);
return es;
if(lag == 666){
lag = forwardHistory.size() - 1;
}
// compute weights using the transition model
// transitionWeigths[numRealizations][numParticles]
std::vector<std::vector<double>> transitionWeights = transition->transition(forwardParticles, backwardParticles.back());
//check if we have enough data for lag
if(forwardHistory.size() <= lag){
lag = forwardHistory.size() - 1;
}
//get the next trajectorie for a realisation
for(int j = 0; j < numRealizations; ++j){
//iterate through all forward filtering steps
for(int i = 0; i <= lag; ++i){
std::vector<Particle<State>> forwardParticles = forwardHistory.getParticleSet(i);
//vector for the current smoothedWeights at time t
std::vector<Particle<State>> smoothedWeights;
smoothedWeights.resize(forwardParticles.size());
smoothedWeights = forwardParticles;
//storage for single trajectories / smoothed particles
smoothedParticles.clear();
//check if all transitionWeights are zero
double weightSumTransition = std::accumulate(transitionWeights[j].begin(), transitionWeights[j].end(), 0.0);
Assert::isNot0(weightSumTransition, "all transition weights for smoothing are zero");
// Choose \tilde x_T = x^(i)_T with probability w^(i)_T
// Therefore sample independently from the categorical distribution of weights.
if(firstFunctionCall){
int i = 0;
for (auto& w : transitionWeights.at(j)) {
smoothedParticles = sampler->drawTrajectorie(forwardParticles, numRealizations);
// multiply the weight of the particles at time t and normalize
smoothedWeights.at(i).weight = (smoothedWeights.at(i).weight * w);
if(smoothedWeights.at(i).weight != smoothedWeights.at(i).weight) {throw "detected NaN";}
firstFunctionCall = false;
backwardParticles.push_back(smoothedParticles);
// iter
++i;
State est = estimation->estimate(smoothedParticles);
estimatedStates.push_back(est);
}
//get the sum of all weights
auto lambda = [](double current, const Particle<State>& a){return current + a.weight; };
double weightSumSmoothed = std::accumulate(smoothedWeights.begin(), smoothedWeights.end(), 0.0, lambda);
// compute weights using the transition model
// transitionWeigths[numRealizations][numParticles]
std::vector<std::vector<double>> transitionWeights = transition->transition(forwardParticles, backwardParticles.back());
//normalize the weights
if(weightSumSmoothed != 0.0){
for (int i = 0; i < smoothedWeights.size(); ++i){
smoothedWeights.at(i).weight /= weightSumSmoothed;
//get the next trajectorie for a realisation
for(int j = 0; j < numRealizations; ++j){
//vector for the current smoothedWeights at time t
std::vector<Particle<State>> smoothedWeights;
smoothedWeights.resize(forwardParticles.size());
smoothedWeights = forwardParticles;
//check if all transitionWeights are zero
double weightSumTransition = std::accumulate(transitionWeights[j].begin(), transitionWeights[j].end(), 0.0);
Assert::isNot0(weightSumTransition, "all transition weights for smoothing are zero");
int k = 0;
for (auto& w : transitionWeights.at(j)) {
// multiply the weight of the particles at time t and normalize
smoothedWeights.at(k).weight = (smoothedWeights.at(k).weight * w);
if(smoothedWeights.at(k).weight != smoothedWeights.at(k).weight) {throw "detected NaN";}
// iter
++k;
}
//check if normalization worked
double normWeightSum = std::accumulate(smoothedWeights.begin(), smoothedWeights.end(), 0.0, lambda);
Assert::isNear(normWeightSum, 1.0, 0.001, "Smoothed weights do not sum to 1");
//get the sum of all weights
auto lambda = [](double current, const Particle<State>& a){return current + a.weight; };
double weightSumSmoothed = std::accumulate(smoothedWeights.begin(), smoothedWeights.end(), 0.0, lambda);
//normalize the weights
if(weightSumSmoothed != 0.0){
for (int l = 0; l < smoothedWeights.size(); ++l){
smoothedWeights.at(l).weight /= weightSumSmoothed;
}
//check if normalization worked
double normWeightSum = std::accumulate(smoothedWeights.begin(), smoothedWeights.end(), 0.0, lambda);
Assert::isNear(normWeightSum, 1.0, 0.001, "Smoothed weights do not sum to 1");
}
//draw the next trajectorie at time t for a realization and save them
smoothedParticles.push_back(sampler->drawSingleParticle(smoothedWeights));
//throw if weight of smoothedParticle is zero
//in practice this is possible, if a particle is completely separated from the rest and is therefore
//weighted zero or very very low.
Assert::isNot0(smoothedParticles.back().weight, "smoothed particle has zero weight");
}
//draw the next trajectorie at time t for a realization and save them
smoothedParticles.push_back(sampler->drawSingleParticle(smoothedWeights));
if(resampler)
{
//TODO - does this even make sense?
Assert::doThrow("Warning - Resampling is not yet implemented!");
}
// push_back the smoothedParticles
backwardParticles.push_back(smoothedParticles);
// estimate the current state
if(lag == (forwardHistory.size() - 1) ){ //fixed lag
State est = estimation->estimate(smoothedParticles);
estimatedStates.push_back(est);
}
else if (i == lag) { //fixed interval
State est = estimation->estimate(smoothedParticles);
estimatedStates.push_back(est);
}
//throw if weight of smoothedParticle is zero
//in practice this is possible, if a particle is completely separated from the rest and is therefore
//weighted zero or very very low.
Assert::isNot0(smoothedParticles.back().weight, "smoothed particle has zero weight");
}
//return the calculated estimations
// TODO: Wir interessieren uns beim fixed-lag smoothing immer nur für die letzte estimation und den letzten satz gesmoothet particle da wir ja weiter vorwärts in der Zeit gehen
// und pro zeitschritt ein neues particle set hinzu kommt. also wenn lag = 3, dann smoothen wir t - 3 und sollten auch nur die estimation von t-3 und das particle set von t-3 abspeichern
//
// beim interval smoothing dagegen interessieren uns alle, da ja keine neuen future informationen kommen und wir einfach sequentiell zurück in die zeit wandern.
//
// lösungsvorschlag.
// - observer-pattern? immer wenn eine neue estimation und ein neues particle set kommt, sende. (wird nix bringen, da keine unterschiedlichen threads?)
// - wie vorher machen, also pro update eine estimation aber jedem update einfach alle observations und alle controls mitgeben?!?!?
// - ich gebe zusätzlich den lag mit an. dann kann die forwardfilterhistory auch ständig alles halten. dann gibt es halt keine ständigen updates, sondern man muss die eine
// berechnung abwarten. eventl. eine art ladebalken hinzufügen. (30 von 120 timestamps done) (ich glaube das ist die beste blackboxigste version) man kann den lag natürlich auch beim
// init des backwardsimulation objects mit übergeben. ABER: damit könntem an kein dynamic-lag smoothing mehr machen. also lieber variable lassen :). durch den lag wissen wir einfach was wir
// genau in estimatedStates und backwardParticles speichern müssen ohne über das problem oben zu stoßen. haben halt keine ständigen updates. observer-pattern hier nur bei mehrere threads,
// das wäre jetzt aber overkill und deshalb einfach ladebalken :):):):)
// - oder man macht einfach zwei update funktionen mit den beiden möglichkeiten. halte ich aber für nen dummen kompromiss. )
if(resampler)
{
// würde es sinn machen, die estimations auch mit zu speichern?
//TODO - does this even make sense?
std::cout << "Warning - Resampling is not yet implemented!" << std::endl;
// //resampling if necessery
// double sum = 0.0;
// double weightSum = std::accumulate(smoothedParticles.begin().weight, smoothedParticles.end().weight, 0.0);
// for (auto& p : smoothedParticles) {
// p.weight /= weightSum;
// sum += (p.weight * p.weight);
// }
// const double neff = 1.0/sum;
// if (neff != neff) {throw "detected NaN";}
// // if the number of efficient particles is too low, perform resampling
// if (neff < smoothedParticles.size() * nEffThresholdPercent) { resampler->resample(smoothedParticles); }
}
// push_back the smoothedParticles
backwardParticles.push_back(smoothedParticles);
// estimate the current state
const State est = estimation->estimate(smoothedParticles);
// done
return est;
return estimatedStates.back();
}
std::vector<State> getEstimations(){
return estimatedStates;
}
};
}