Beispiel #1
0
int main(){
	
	const int length = 5;
	float input[length] = {-0.665365, -0.329988, 0.164465, 0.043962, 0.295885};
	
	float output[length];           	// array for storing Kalman processed values
	float temp[length];             	// array for storing subtraction results
	float temp2[length];	
	float miscresult[2] = {0, 0};   	// array for storing mean and std dev results
	float holder[length];           	// array for storing convolution results
	int i, j;
	float corr_temp[1] = {0};

	/*START OF PART II*/
	kalman_state kstate;
	reset(&kstate);
	//Kalmanfilter_C(input, output, &kstate, length);
	Kalmanfilter_asm(output, input, length, &kstate);
	printf("\n");
	/*END OF PART II*/

	/*START OF PART III*/	
	// subtract
	printf("subtraction:\n");
	subtract(temp, input, output, length);
	arm_sub_f32(input, output, temp2, length);
	for(j = 0; j < length; j++){
		printf("My implementation: %f CMSIS: %f\n", temp[j], temp2[j]);
	}

	// misc
	printf("\n");
	//misc(miscresult, temp, length);
	arm_mean_f32(temp, length, &miscresult[0]);
	arm_std_f32(temp, length, &miscresult[1]);
	printf("mean: %f stdev: %f\n", miscresult[0], miscresult[1]);

	// correlation
	//corr_temp[0] = correlation(input, output, length);
	arm_correlate_f32(input, length, output, length, &corr_temp[0]);
	printf("correlation: %f\n", corr_temp[0]);
	

	// convolution
	printf("\n");
	for(i = 0; i < length; i++){
		holder[i] = 0;
	}
	convolve(holder, input, output, length);
	//arm_conv_f32(input, length, output, length, holder);
	for(i = 0; i < length; i++){
		printf("convolution %f \n", holder[i]);
	}
	/*END OF PART III*/

	return 0;
}
int32_t main() 
{ 
 
#ifndef  USE_STATIC_INIT 
 
  	arm_matrix_instance_f32 srcA; 
  	arm_matrix_instance_f32 srcB; 
  	arm_matrix_instance_f32 dstC;  
 
	/* Input and output matrices initializations */  
	arm_mat_init_f32(&srcA, numStudents, numSubjects, (float32_t *)testMarks_f32);  
	arm_mat_init_f32(&srcB, numSubjects, 1, (float32_t *)testUnity_f32);  
	arm_mat_init_f32(&dstC, numStudents, 1, testOutput);  
 
#else 
 
	/* Static Initializations of Input and output matrix sizes and array */ 
	arm_matrix_instance_f32 srcA = {NUMSTUDENTS, NUMSUBJECTS, (float32_t *)testMarks_f32}; 
	arm_matrix_instance_f32 srcB = {NUMSUBJECTS, 1, (float32_t *)testUnity_f32}; 
	arm_matrix_instance_f32 dstC = {NUMSTUDENTS, 1, testOutput}; 
 
#endif 
 
	 
	/* ---------------------------------------------------------------------- 
	*Call the Matrix multiplication process function   
	* ------------------------------------------------------------------- */ 
	arm_mat_mult_f32(&srcA, &srcB, &dstC); 
	 
	/* ---------------------------------------------------------------------- 
	** Call the Max function to calculate max marks among numStudents 
	** ------------------------------------------------------------------- */ 
	arm_max_f32(testOutput, numStudents, &max_marks, &student_num);  
 
	/* ---------------------------------------------------------------------- 
	** Call the Min function to calculate min marks among numStudents 
	** ------------------------------------------------------------------- */ 
	arm_min_f32(testOutput, numStudents, &min_marks, &student_num);  
 
	/* ---------------------------------------------------------------------- 
	** Call the Mean function to calculate mean 
	** ------------------------------------------------------------------- */ 
	arm_mean_f32(testOutput, numStudents, &mean); 
 
	/* ---------------------------------------------------------------------- 
	** Call the std function to calculate standard deviation 
	** ------------------------------------------------------------------- */ 
	arm_std_f32(testOutput, numStudents, &std); 
 
	/* ---------------------------------------------------------------------- 
	** Call the var function to calculate variance 
	** ------------------------------------------------------------------- */ 
	arm_var_f32(testOutput, numStudents, &var); 
 
    while(1);                             /* main function does not return */
} 
Beispiel #3
0
float FloatArray::getStandardDeviation(){
  float result;
/// @note When built for ARM Cortex-M processor series, this method uses the optimized <a href="http://www.keil.com/pack/doc/CMSIS/General/html/index.html">CMSIS library</a>
#ifdef ARM_CORTEX  
  arm_std_f32 (data, size, &result);
#else
  result=sqrtf(getVariance());
#endif
  return result;
}
Beispiel #4
0
// ARM DSP Function ****************************************************************************************
int dsp_arm(float* inputArray, float* outputArray, dsp_t* analysisOut) {
	
	// Get diffence Array
	arm_sub_f32(outputArray, inputArray, analysisOut->diffArr, ARRAY_LENGTH);
	
	// Get mean
	arm_mean_f32(analysisOut->diffArr, (uint32_t) ARRAY_LENGTH, &analysisOut->meanDiff);
	
	// Get standard deviation
	arm_std_f32(analysisOut->diffArr, (uint32_t) ARRAY_LENGTH, &analysisOut->standDevDiff);
	
	// Get correlation
	arm_correlate_f32(inputArray, ARRAY_LENGTH, outputArray, ARRAY_LENGTH, analysisOut->corrArr);
	
	// Get convolution
	arm_conv_f32(inputArray, ARRAY_LENGTH, outputArray, ARRAY_LENGTH, analysisOut->convolArr);
	
	
	return 0;
}
Beispiel #5
0
void runTaskCode(void *unused) {
    uint32_t axis = 0;
    uint32_t loops = 0;

    AQ_NOTICE("Run task started\n");

    while (1) {
	// wait for data
	CoWaitForSingleFlag(imuData.sensorFlag, 0);

	// soft start GPS accuracy
	runData.accMask *= 0.999f;

	navUkfInertialUpdate();

	// record history for acc & mag & pressure readings for smoothing purposes
	// acc
	runData.sumAcc[0] -= runData.accHist[0][runData.sensorHistIndex];
	runData.sumAcc[1] -= runData.accHist[1][runData.sensorHistIndex];
	runData.sumAcc[2] -= runData.accHist[2][runData.sensorHistIndex];

	runData.accHist[0][runData.sensorHistIndex] = IMU_ACCX;
	runData.accHist[1][runData.sensorHistIndex] = IMU_ACCY;
	runData.accHist[2][runData.sensorHistIndex] = IMU_ACCZ;

	runData.sumAcc[0] += runData.accHist[0][runData.sensorHistIndex];
	runData.sumAcc[1] += runData.accHist[1][runData.sensorHistIndex];
	runData.sumAcc[2] += runData.accHist[2][runData.sensorHistIndex];

	// mag
	runData.sumMag[0] -= runData.magHist[0][runData.sensorHistIndex];
	runData.sumMag[1] -= runData.magHist[1][runData.sensorHistIndex];
	runData.sumMag[2] -= runData.magHist[2][runData.sensorHistIndex];

	runData.magHist[0][runData.sensorHistIndex] = IMU_MAGX;
	runData.magHist[1][runData.sensorHistIndex] = IMU_MAGY;
	runData.magHist[2][runData.sensorHistIndex] = IMU_MAGZ;

	runData.sumMag[0] += runData.magHist[0][runData.sensorHistIndex];
	runData.sumMag[1] += runData.magHist[1][runData.sensorHistIndex];
	runData.sumMag[2] += runData.magHist[2][runData.sensorHistIndex];

	// pressure
	runData.sumPres -= runData.presHist[runData.sensorHistIndex];
	runData.presHist[runData.sensorHistIndex] = AQ_PRESSURE;
	runData.sumPres += runData.presHist[runData.sensorHistIndex];

	runData.sensorHistIndex = (runData.sensorHistIndex + 1) % RUN_SENSOR_HIST;

	if (!((loops+1) % 20)) {
	   simDoAccUpdate(runData.sumAcc[0]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumAcc[1]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumAcc[2]*(1.0f / (float)RUN_SENSOR_HIST));
	}
	else if (!((loops+7) % 20)) {
	   simDoPresUpdate(runData.sumPres*(1.0f / (float)RUN_SENSOR_HIST));
	}
#ifndef USE_DIGITAL_IMU
	else if (!((loops+13) % 20) && AQ_MAG_ENABLED) {
	   simDoMagUpdate(runData.sumMag[0]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumMag[1]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumMag[2]*(1.0f / (float)RUN_SENSOR_HIST));
	}
#endif
	// optical flow update
	else if (navUkfData.flowCount >= 10 && !navUkfData.flowLock) {
	    navUkfFlowUpdate();
	}
	// only accept GPS updates if there is no optical flow
	else if (CoAcceptSingleFlag(gpsData.gpsPosFlag) == E_OK && navUkfData.flowQuality == 0.0f && gpsData.hAcc < NAV_MIN_GPS_ACC && gpsData.tDOP != 0.0f) {
	    navUkfGpsPosUpdate(gpsData.lastPosUpdate, gpsData.lat, gpsData.lon, gpsData.height, gpsData.hAcc + runData.accMask, gpsData.vAcc + runData.accMask);
	    CoClearFlag(gpsData.gpsPosFlag);
	    // refine static sea level pressure based on better GPS altitude fixes
	    if (gpsData.hAcc < runData.bestHacc && gpsData.hAcc < NAV_MIN_GPS_ACC) {
                navPressureAdjust(gpsData.height);
		runData.bestHacc = gpsData.hAcc;
	    }
	}
	else if (CoAcceptSingleFlag(gpsData.gpsVelFlag) == E_OK && navUkfData.flowQuality == 0.0f && gpsData.sAcc < NAV_MIN_GPS_ACC/2 && gpsData.tDOP != 0.0f) {
	    navUkfGpsVelUpdate(gpsData.lastVelUpdate, gpsData.velN, gpsData.velE, gpsData.velD, gpsData.sAcc + runData.accMask);
	    CoClearFlag(gpsData.gpsVelFlag);
	}
	// observe zero position
	else if (!((loops+4) % 20) && (gpsData.hAcc >= NAV_MIN_GPS_ACC || gpsData.tDOP == 0.0f) && navUkfData.flowQuality == 0.0f) {
	    navUkfZeroPos();
	}
	// observer zero velocity
	else if (!((loops+10) % 20) && (gpsData.sAcc >= NAV_MIN_GPS_ACC/2 || gpsData.tDOP == 0.0f) && navUkfData.flowQuality == 0.0f) {
	    navUkfZeroVel();
	}
	// observe that the rates are exactly 0 if not flying or moving
	else if (!(supervisorData.state & STATE_FLYING)) {
	    float stdX, stdY, stdZ;

	    arm_std_f32(runData.accHist[0], RUN_SENSOR_HIST, &stdX);
	    arm_std_f32(runData.accHist[1], RUN_SENSOR_HIST, &stdY);
	    arm_std_f32(runData.accHist[2], RUN_SENSOR_HIST, &stdZ);

	    if ((stdX + stdY + stdZ) < (IMU_STATIC_STD*2)) {
		if (!((axis + 0) % 3))
		    navUkfZeroRate(IMU_RATEX, 0);
		else if (!((axis + 1) % 3))
		    navUkfZeroRate(IMU_RATEY, 1);
		else
		    navUkfZeroRate(IMU_RATEZ, 2);
		axis++;
	    }
	}

        navUkfFinish();
        altUkfProcess(AQ_PRESSURE);

        // determine which altitude estimate to use
        if (gpsData.hAcc > p[NAV_ALT_GPS_ACC]) {
            runData.altPos = &ALT_POS;
            runData.altVel = &ALT_VEL;
        }
        else {
            runData.altPos = &UKF_ALTITUDE;
            runData.altVel = &UKF_VELD;
        }

	CoSetFlag(runData.runFlag);	// new state data

	navNavigate();
#ifndef HAS_AIMU
	analogDecode();
#endif
	if (!(loops % (int)(1.0f / AQ_OUTER_TIMESTEP)))
	    loggerDoHeader();
	loggerDo();
	gimbalUpdate();

#ifdef CAN_CALIB
	canTxIMUData(loops);
#endif
        calibrate();

	loops++;
    }
}
int main()
{
	//initialize testing array
	float testVector[] = {0.1f,0.2f,0.3f,0.4f,0.5f};
	
	/*COMMENTED OUT LENGTH PARAM AS IT IS INCLUDED IN HEADER FILE*/
	//get the size of the array
	//int length = sizeof(testVector)/sizeof(float);

	//initiate empty output array of size length
	float outputArrayC[length];
	//initialize the struct at p=r=q 0.1 and x=k=0 
	kalman_state currentState = {0.1f, 0.1f, 0.0f , 0.1f, 0.0f};
	//call function Kalmanfilter_C
	Kalmanfilter_C(measurements, outputArrayC, &currentState, length);
	
	//initiate empty output array of size length
	float outputArrayASM[length];
	
	//reinitialize the struct at p=r=q 0.1 and x=k=0 
	currentState.p = DEF_p; 
	currentState.r = DEF_r; 	
	currentState.k = DEF_k;
	currentState.q = DEF_q; 
	currentState.x = DEF_x; 
	
	//call subroutine Kalmanfilter_asm
	Kalmanfilter_asm(measurements, outputArrayASM, &currentState, length );

	//Check for correctness with a error tolerance of 0.000001
	float errorTolerance = 0.000001f;
	float errorPercentage = 0.01;

	//is_valid(outputArrayC, outputArrayASM, length, errorTolerance, "c vs asm");
	//is_valid_relative(outputArrayC, outputArrayASM, length, errorTolerance, errorPercentage,"c vs asm");

	int p;

	//print KalmanFilter output
	for ( p = 0; p < length;  p++ )
	{
		printf("OutputASM: %f & OutputC %f\n", outputArrayASM[p], outputArrayC[p]); 
	}

	float differenceC[length];
	float differenceCMSIS[length];
	
	//Difference
	arm_sub_f32 (measurements, outputArrayC, differenceCMSIS, length);
	c_sub(measurements, outputArrayC, differenceC, length);
	
	//is_valid(differenceC, differenceCMSIS, length, errorTolerance, "Difference");
	//is_valid_relative(differenceC, differenceCMSIS, length, errorTolerance, errorPercentage,"Difference");
	
	//Print difference vector
	for ( p = 0; p < length;  p++ )
	{
		printf("DifferenceC: %f & DifferenceCMSIS %f \n", differenceC[p], differenceCMSIS[p]); 
	}


	//Mean
	float meanCMSIS;
	float meanC; 
	arm_mean_f32 (differenceCMSIS, length , &meanCMSIS);
	c_mean(differenceC,length, &meanC); 
	//is_valid(&meanC, &meanCMSIS, 1, errorTolerance, "mean"); 
	//is_valid_relative(&meanC, &meanCMSIS, 1, errorTolerance, errorPercentage, "mean"); 
	
	//Print mean values
	printf("MeanC: %f & MeanCMSIS %f \n", meanC, meanCMSIS); 
	
	//STD
	float stdC;
	float stdCMSIS;
	arm_std_f32 (differenceCMSIS, length, &stdCMSIS);
	c_std(differenceC, length, &stdC);
	//is_valid(&stdC, &stdCMSIS, 1, errorTolerance, "STD");
	//is_valid_relative(&stdC, &stdCMSIS, 1, errorTolerance, errorPercentage,"STD");

	//Print std values
	printf("StandardDevC: %f & StandardDevCMSIS %f \n", stdC, stdCMSIS); 

	//correlation
	float corC[2*length-1];
	float corCMSIS[2*length-1];
	arm_correlate_f32 (measurements, length, outputArrayC, length, corCMSIS);
	c_correlate(measurements, outputArrayC, corC, length);
	//is_valid(corC, corCMSIS, 2*length-1, errorTolerance, "correlation"); 
	//is_valid_relative(corC, corCMSIS, 2*length-1, errorTolerance, errorPercentage, "correlation"); 
	
	//convolution
	float convC[2*length-1];
	float convCMSIS[2*length-1];
	arm_conv_f32 (measurements, length, outputArrayC, length, convCMSIS);
	c_conv(measurements, outputArrayC, convC, length);
	//is_valid(convC, convCMSIS, 2*length-1, errorTolerance, "convolution"); 
	//is_valid_relative(convC, convCMSIS, 2*length-1, errorTolerance, errorPercentage, "convolution"); 

	//Print correlation and convolution values
	for ( p = 0; p < (2*length-1);  p++ )
	{
		printf("ConvC: %f & ConvCMSIS: %f \n", convC[p], convCMSIS[p]); 
	}
	for ( p = 0; p < (2*length-1);  p++ )
	{
		printf("CorrelateC: %f & CorrelatCMSIS: %f \n", corC[p], corCMSIS[p]); 
	}

	return 0;
}