comparison src/BayesianArrayStructure.cpp @ 4:4a8e6a6cd224

optimised draw function in dynamic vector class. Added Gaussian lookup but not yet used.

 	 tmpPrior.addGaussianShape(100, 40, 1);
 	 tmpPrior.addGaussianShape(200, 10, 0.2);
 	 tmpPrior.translateDistribution(20);
 	 */
 	tmpBestEstimate = 0;
+	crossUpdateTimeThreshold = 100;
 }
 
 BayesianArrayStructure::BayesianArrayStructure(int length){
 	printf("BAYESIAN STURTUCRE CREATED LENGTH: %i\n", length);
 	//this constructor isnt called  it seems

 	
 //	printf("time difference %f, ", timeDifference);
 	
 	double timeDifferenceInPositionVectorUnits = timeDifference / prior.scalar;
 	
+
 	prior.zero();//kill prior
 	calculateNewPriorOffset(timeDifference);//set new prior offset here
 	
-	for (int i = 0;i < speed.arraySize;i++){
-//		printf("[%i] %f\n", i, speed.array[i]);
-		//set speed
-		double speedValue = speed.getIndexInRealTerms(i);//so for scalar 0.01, 50 -> speed value of 0.5
-
+	if (timeDifferenceInPositionVectorUnits > crossUpdateTimeThreshold)
+		complexCrossUpdate(timeDifferenceInPositionVectorUnits);
+	else
+		translateByMaximumSpeed(timeDifferenceInPositionVectorUnits);	
+			
+
+	updateCounter++;
+	prior.renormalise();
+
+}
+
+void BayesianArrayStructure::complexCrossUpdate(const double& timeDifferenceInPositionVectorUnits){
+	int distanceMoved, newPriorIndex;
+	for (int i = 0;i < relativeSpeedPosterior.arraySize;i++){
+		
+		double speedValue = relativeSpeedPosterior.getIndexInRealTerms(i);//so for scalar 0.01, 50 -> speed value of 0.5
+		
 		//so we have moved 
-		int distanceMoved = round(timeDifferenceInPositionVectorUnits * speedValue);//round the value
-		
-		if (speed.array[i] != 0){
+		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);
 			
-		//	printf("speed [%i] gives %f moved %i in %f units \n", i, speedValue, distanceMoved, timeDifferenceInPositionVectorUnits);
-			
-		for (int postIndex = 0;postIndex < position.arraySize;postIndex++){
-			//old posterior contributing to new prior
-			int newPriorIndex = postIndex + position.offset - prior.offset + distanceMoved;
-			if (newPriorIndex >= 0 && newPriorIndex < prior.arraySize){
-				prior.addToIndex(newPriorIndex, position.array[postIndex]*speed.array[i]);
-			//	printf("adding [%i] : %f\n", newPriorIndex, posterior.array[postIndex]*speed.array[i]);
+			for (int postIndex = 0;postIndex < posterior.arraySize;postIndex++){
+				//old posterior contributing to new prior
+				newPriorIndex = postIndex + posterior.offset - prior.offset + distanceMoved;
+				if (newPriorIndex >= 0 && newPriorIndex < prior.arraySize){
+					prior.addToIndex(newPriorIndex, posterior.array[postIndex]*speedContribution);
+				}
+				
 			}
-		
-		}
 			
 		}//if not zero
 	}//end speed
-	updateCounter++;
-	prior.renormalise();
-
-}
-
+}
+
+
+
+void BayesianArrayStructure::translateByMaximumSpeed(const double& timeDifferenceInPositionVectorUnits){
+	int distanceMoved, newPriorIndex;
+
+		
+		double speedValue = relativeSpeedPosterior.getIndexInRealTerms(relativeSpeedPosterior.MAPestimate);//using max value only
+	//so for scalar 0.01, 50 -> speed value of 0.5
+	double speedContribution = relativeSpeedPosterior.array[relativeSpeedPosterior.MAPestimate];
+		//so we have moved 
+		distanceMoved = round(timeDifferenceInPositionVectorUnits * speedValue);//round the value
+					//	printf("speed [%i] gives %f moved %i in %f units \n", i, speedValue, distanceMoved, timeDifferenceInPositionVectorUnits);
+			
+			for (int postIndex = 0;postIndex < posterior.arraySize;postIndex++){
+				//old posterior contributing to new prior
+				newPriorIndex = postIndex + posterior.offset - prior.offset + distanceMoved;
+				if (newPriorIndex >= 0 && newPriorIndex < prior.arraySize){
+					prior.addToIndex(newPriorIndex, posterior.array[postIndex]*speedContribution);
+				}
+				
+			}
+	
+}
 
 void BayesianArrayStructure::addGaussianNoiseToSpeedPosterior(const double& std_dev){
 	tmpPosteriorForStorage.copyFromDynamicVector(relativeSpeedPosterior);
 	
 	for (int i = 0;i < relativeSpeedPosterior.length;i++){