diff src/BayesianArrayStructure.cpp @ 4:4a8e6a6cd224

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

--- a/src/BayesianArrayStructure.cpp	Fri Aug 19 02:36:34 2011 +0100
+++ b/src/BayesianArrayStructure.cpp	Fri Aug 19 15:53:04 2011 +0100
@@ -30,6 +30,7 @@
 	 tmpPrior.translateDistribution(20);
 	 */
 	tmpBestEstimate = 0;
+	crossUpdateTimeThreshold = 100;
 }
 
 BayesianArrayStructure::BayesianArrayStructure(int length){
@@ -250,38 +251,70 @@
 	
 	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);	
+			
 
-		//so we have moved 
-		int distanceMoved = round(timeDifferenceInPositionVectorUnits * speedValue);//round the value
-		
-		if (speed.array[i] != 0){
-			
-		//	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]);
-			}
-		
-		}
-			
-		}//if not zero
-	}//end speed
 	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 
+		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);
+			
+			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
+}
+
+
+
+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);