comparison src/BayesianArrayStructure.cpp @ 3:de86d77f2612

added speed prior setting. needs some cleaning

  *  Created by Andrew on 17/07/2011.
  *  Copyright 2011 QMUL. All rights reserved.
  *
  */
 
+//look at reset speed to one - what does this do? - get rid of?
+
+
 #include "BayesianArrayStructure.h"
 
 BayesianArrayStructure::BayesianArrayStructure(){
 	printf("Bayesian structure: DeFault constructor called");
 	
 	prior.createVector(1);
 	likelihood.createVector(1);
 	posterior.createVector(1);
 
-
+	speedPriorValue = 1.0;
+	
 	lastEventTime = ofGetElapsedTimeMillis();
 	
 	/*
 	 tmpPrior.createVector(240);
 	 tmpPrior.addGaussianShape(100, 40, 1);

 //	relativeSpeedPosterior.addToIndex(100, 1);
 	//relativeSpeedPosterior.addToIndex(50, 1);
 	relativeSpeedPosterior.renormalise();
 	relativeSpeedPosterior.getMaximum();
 	
+	setSpeedPrior(speedPriorValue);
+	
 	prior.zero();
 	posterior.zero();
 //	posterior.offset = - 200;
 //	posterior.addGaussianShape(200, 40, 1);
 	posterior.addToIndex(0, 1);

 	
 	//acceleration.addGaussianShape(2000, 20, 0.8);
 	
 }
 
+void BayesianArrayStructure::setSpeedPrior(double f){	
+	speedPriorValue = f;
+	int index = relativeSpeedPosterior.getRealTermsAsIndex(speedPriorValue);
+	relativeSpeedPosterior.zero();
+	relativeSpeedPosterior.addGaussianShape(index, 20, 0.8);
+	relativeSpeedPosterior.renormalise();
+	relativeSpeedPosterior.getMaximum();
+	relativeSpeedPrior.copyFromDynamicVector(relativeSpeedPosterior);
+	printf("BAYES STRUCTU ' SPEED PRIOR %f . index %i\n", speedPriorValue, index);
+	
+}
+
 void BayesianArrayStructure::resetSpeedSize(int length){
 	printf("BAYESIAN SPEED size is : %i\n", length);
 	
 	relativeSpeedPrior.createVector(length);
 	relativeSpeedLikelihood.createVector(length);

 	updateCounter = 0;
 	
 	bestEstimate = 0;
 //	lastBestEstimateUpdateTime = ofGetElapsedTimeMillis();
 	
+	setSpeedPrior(speedPriorValue);
 }
 
 void BayesianArrayStructure::zeroArrays(){
 	prior.zero();
 	likelihood.zero();

 	
 //	double timeDiff = ofGetElapsedTimeMillis() - lastEventTime;//lastBestEstimateUpdateTime;
 	
 	tmpBestEstimate = posterior.getIndexInRealTerms(posterior.MAPestimate) + timeDifference*relativeSpeedPosterior.getIndexInRealTerms(relativeSpeedPosterior.MAPestimate);
 	// 
-	printf("tmp best %f and best %f time diff %f posterior MAP %f\n", tmpBestEstimate, bestEstimate, timeDifference, posterior.getIndexInRealTerms(posterior.MAPestimate));
+	printf("tmp best %f and best %f time diff %f posterior MAP %f at speed %f\n", tmpBestEstimate, bestEstimate, timeDifference, posterior.getIndexInRealTerms(posterior.MAPestimate), relativeSpeedPosterior.getIndexInRealTerms(relativeSpeedPosterior.MAPestimate));
 	//lastBestEstimateUpdateTime = ofGetElapsedTimeMillis();
 }
 	
 void BayesianArrayStructure::updateBestEstimate(){
 //	double timeDiff = ofGetElapsedTimeMillis() - lastEventTime;//