comparison bayesianArraySrc/BayesianArrayStructure.cpp @ 4:45b5cf9be377

checking through Bayesian update procedure - working without cross update method.

 #include "BayesianArrayStructure.h"
 
 BayesianArrayStructure::BayesianArrayStructure(){
 	printf("Bayesian structure: DeFault constructor called");
 
-	usingIntegratedTempoEstimate = true;// use max index
+	usingIntegratedTempoEstimate = false;// use max index
 	
 	relativeSpeedLikelihoodStdDev = 5.0;
 	
 	prior.createVector(1);
 	likelihood.createVector(1);

 	speedEstimate = speedPriorValue;
 	
 	lastEventTime = 0;//ofGetElapsedTimeMillis();
 
 	tmpBestEstimate = 0;
-	crossUpdateTimeThreshold = 60;
+	crossUpdateTimeThreshold = 60000;
 	priorWidth = 20;
 	
 }
 
 BayesianArrayStructure::BayesianArrayStructure(int length){

 void BayesianArrayStructure::setSpeedPrior(double f){	
 	speedPriorValue = f;
 	int index = relativeSpeedPosterior.getRealTermsAsIndex(speedPriorValue);
 	relativeSpeedPosterior.zero();
 	relativeSpeedPosterior.addGaussianShape(index, priorWidth, 0.8);
+	printf("speed adding to index for 1 = %f\n", relativeSpeedPosterior.getRealTermsAsIndex(1));
+	relativeSpeedPosterior.addToIndex(relativeSpeedPosterior.getRealTermsAsIndex(1), 1);
+	
 	relativeSpeedPosterior.renormalise();
 	relativeSpeedPosterior.getMaximum();
 	relativeSpeedPrior.copyFromDynamicVector(relativeSpeedPosterior);
 	printf("BAYES STRUCTU ' SPEED PRIOR %f . index %i\n", speedPriorValue, index);
 	

 	relativeSpeedPosterior.zero();
 	relativeSpeedLikelihood.zero();
 
 }
 
-
+/*
 void BayesianArrayStructure::updateTmpBestEstimate(const double& timeDifference){
 	//input is the time since the start of playing
 //	double timeDiff = ofGetElapsedTimeMillis() - lastEventTime;//lastBestEstimateUpdateTime;
 	double timeDiff = timeDifference;
 	if (*realTimeMode)

 	tmpBestEstimate = posterior.getIndexInRealTerms(posterior.MAPestimate) + timeDiff*relativeSpeedPosterior.getIndexInRealTerms(relativeSpeedPosterior.integratedEstimate);
 	// 
 	//printf("tmp best %f and best %f time diff %f posterior MAP %f at speed %f\n", 0Estimate, bestEstimate, timeDifference, posterior.getIndexInRealTerms(posterior.MAPestimate), relativeSpeedPosterior.getIndexInRealTerms(relativeSpeedPosterior.integratedEstimate));
 	//lastBestEstimateUpdateTime = ofGetElapsedTimeMillis();
 }
-	
+*/
+
 void BayesianArrayStructure::updateBestEstimate(const double& timeDifference){
 //	double timeDiff = ofGetElapsedTimeMillis() - lastEventTime;//
 	double tmp = bestEstimate;
-	printf("post offest %i\n", posterior.offset);
+	printf("best est routine: posterior offset %f\n", posterior.offset);
 	
 	double timeDiff = timeDifference;
 	
 	//Using timedifferencfe here will make it go wrong. Is time since beginning of playing
 	
 	if (*realTimeMode)
 		timeDiff = ofGetElapsedTimeMillis() - lastBestEstimateUpdateTime;
 	
 	//lastbest is time we started playing
-	
+	/*
 	if (usingIntegratedTempoEstimate)
 		speedEstimateIndex = relativeSpeedPosterior.getIntegratedEstimate();
 	else
-		speedEstimateIndex = relativeSpeedPosterior.MAPestimate;
+		speedEstimateIndex = relativeSpeedPosterior.getMAPestimate();
+	*/
+	speedEstimateIndex = getSpeedEstimateIndex();
 	
 	speedEstimate = relativeSpeedPosterior.getIndexInRealTerms(speedEstimateIndex);
 	bestEstimate = posterior.getIndexInRealTerms(posterior.MAPestimate) + timeDiff*speedEstimate;
 	
-	printf("best estimate update from %f to %f; time diff %f MAP %i = %f ms speed %f\n", tmp, bestEstimate, timeDiff,
-		   posterior.MAPestimate, posterior.getIndexInRealTerms(posterior.MAPestimate), speedEstimate);
+	printf("best estimate update from %f to %f; time diff %f MAP %i = %f ms speed index %f est %f SpeedxTime %f\n", tmp, bestEstimate, timeDiff,
+		   posterior.MAPestimate, posterior.getIndexInRealTerms(posterior.MAPestimate), speedEstimateIndex, speedEstimate, timeDiff*speedEstimate);
 }
 
 void BayesianArrayStructure::calculatePosterior(){
 	//posterior.doProduct(prior, likelihood);
+	assert(posterior.offset == prior.offset);
 	
 	int i;
 	for (i = 0;i < posterior.length;i++){
 		posterior.array[i] = likelihood.array[i] * prior.array[i];
 	}

 	posterior.renormalise();
 	
 }
 
 
+double  BayesianArrayStructure::getSpeedEstimateIndex(){
+	if (usingIntegratedTempoEstimate)
+		return relativeSpeedPosterior.getIntegratedEstimate();
+	else
+		return relativeSpeedPosterior.getMAPestimate();
+}
 
 
 void BayesianArrayStructure::setNewDistributionOffsets(const double& newOffset){
+	
+	printf("prior offset was %f now %f\n", prior.offset, newOffset);
+	
 	prior.offset = newOffset;
 	likelihood.offset = newOffset;
+	
 	//posterior.offset = newOffset;
 }
 
 
 void BayesianArrayStructure::crossUpdateArrays(DynamicVector& position, DynamicVector& speed, double timeDifference){

 	
 //	printf("time difference %f, ", timeDifference);
 	
 	double timeDifferenceInPositionVectorUnits = timeDifference / prior.scalar;
 	
-
+	printf("CROSS UPDATE time diff %f ms is %f units; ", timeDifference, timeDifferenceInPositionVectorUnits);
 	prior.zero();//kill prior
-	calculateNewPriorOffset(timeDifference);//set new prior offset here
+	
+//	calculateNewPriorOffset(timeDifference);//dioesnt do anything
+	
+	printf("new prior offset %f and post offset %f\n", prior.offset, posterior.offset);
 	
 	if (timeDifferenceInPositionVectorUnits > crossUpdateTimeThreshold)
 		complexCrossUpdate(timeDifferenceInPositionVectorUnits);
 	else
 		translateByMaximumSpeed(timeDifferenceInPositionVectorUnits);	

 	prior.renormalise();
 
 }
 
 void BayesianArrayStructure::complexCrossUpdate(const double& timeDifferenceInPositionVectorUnits){
+
 	int distanceMoved, newPriorIndex;
 	
 	double speedValue = relativeSpeedPosterior.offset;
 	
 	for (int i = 0;i < relativeSpeedPosterior.arraySize;i++){

 		
 		//so we have moved 
 		distanceMoved = round(timeDifferenceInPositionVectorUnits * speedValue);//round the value
 	
 		if (relativeSpeedPosterior.array[i] != 0){
+			
 			double speedContribution = relativeSpeedPosterior.array[i];
-			//	printf("speed [%i] gives %f moved %i in %f units \n", i, speedValue, distanceMoved, timeDifferenceInPositionVectorUnits);
-			
-			newPriorIndex = posterior.offset - prior.offset + distanceMoved;
-			
-			for (int postIndex = 0;postIndex < posterior.arraySize;postIndex++){
+			
+		//	printf("speed [%i](val[%f]) gives %f moved %i in %f units \n", i, relativeSpeedPosterior.array[i], speedValue, distanceMoved, timeDifferenceInPositionVectorUnits);
+			
+			//1/2/12 deleted line
+			newPriorIndex = posterior.offset - prior.offset + distanceMoved;//i.e. where post[0] goes to in terms of prior at this speed
+			int postIndex = 0;
+			while (postIndex < posterior.arraySize && newPriorIndex < prior.arraySize){
+
+			//did use a for loop
+			//	for (postIndex = 0;postIndex < posterior.arraySize;postIndex++){
 				//old posterior contributing to new prior
-			//	newPriorIndex = postIndex + posterior.offset - prior.offset + distanceMoved;
+			
+				//would use this method
+				//newPriorIndex = postIndex + posterior.offset - prior.offset + distanceMoved;
 				
-				if (newPriorIndex >= 0 && newPriorIndex < prior.arraySize){
+				if (newPriorIndex >= 0){
 					prior.addToIndex(newPriorIndex, posterior.array[postIndex]*speedContribution);
+			//		printf("speed index %i new prior index %i post val %f speed contrib %f dist %i\n", i, newPriorIndex, posterior.array[postIndex], speedContribution, distanceMoved);
 				}
-				
-				newPriorIndex++;//optimised code - the commented line above explains how this works
-			}//end for
+				//but we actually do this for simplicity
+				newPriorIndex++;
+				postIndex++;
+			}//end for. now while
 			
 			
 		}//if not zero
 		speedValue += relativeSpeedPosterior.scalar;
 		//optimised line

 	relativeSpeedPosterior.copyFromDynamicVector(tmpPosteriorForStorage);											
 }
 
 void BayesianArrayStructure::calculateNewPriorOffset(const double& timeDifference){
 	
-	double maxSpeed = relativeSpeedPosterior.getIndexInRealTerms(relativeSpeedPosterior.integratedEstimate);
+//	double maxSpeed = relativeSpeedPosterior.getIndexInRealTerms(relativeSpeedPosterior.integratedEstimate);
+	
+	double maxSpeed = relativeSpeedPosterior.getIndexInRealTerms(getSpeedEstimateIndex());//either integrated or MAP
 	//	printf("Maxspeed is %f\n", maxSpeed);
 	
 	double priorMax = posterior.getMaximum();
 	double distanceTravelled = maxSpeed * (timeDifference / prior.scalar);
 	double newMaxLocation = posterior.MAPestimate + distanceTravelled;