void readAccelerometerData(void){ float readings[3]; float* calibrated_matrix; float* kalman_output; float Ax, Ay,Az; LIS3DSH_ReadACC(readings); kalman_output = multiplyMatrix(readings); readings[0] = kalmanFilter(readings[0],&kSx); readings[1] = kalmanFilter(readings[1],&kSy); readings[2] = kalmanFilter(readings[2],&kSz); calibrated_matrix = multiplyMatrix(readings); Ax = calibrated_matrix[0]; Ay = calibrated_matrix[1]; Az = calibrated_matrix[2]; Pitch = atan2(Ax,Az) * 180 / 3.1415926515; Roll = atan2(Ay,Az)* 180 / 3.1415926515; //printf("Pitch = %f \n", Pitch); }
// MEMS thread handler void mems(void const * arg){ // Kalman filter state initilization KalmanState kstate; kstate.p = 0.0; kstate.k = 0.0; kstate.r = 50; kstate.q = 0.5; kstate.x = 0.0; float pitch = 0.0; mems_init(); while(1){ osSignalWait(MEMS_READY, osWaitForever); pitch = kalmanFilter(get_pitch_angle(), &kstate); // send a message to the display thread send_message(pitch, pitch_queue); } }
// temperature thread handler void temp_sensor(void const * arg){ // set the initial Kalman filter state. KalmanState kstate; kstate.p = 0.1; kstate.k = 0.0; kstate.r = 2.25; kstate.q = 0.01; kstate.x = 0.0; float current_temperature = 0.0; temp_sensor_init(); while(1){ osSignalWait(TEMP_SENSOR_READY, osWaitForever); // sample the current temperature of the processor. current_temperature = kalmanFilter(volt_to_celsius(getADCVoltage()), &kstate); // send a message to the display thread send_message(current_temperature, temp_queue); } }
int main(void) { cv::KalmanFilter kalmanFilter(4, 2, 0); // instantiate Kalman Filter float fltTransitionMatrixValues[4][4] = { { 1, 0, 1, 0 }, // declare an array of floats to feed into Kalman Filter Transition Matrix, also known as State Transition Model { 0, 1, 0, 1 }, { 0, 0, 1, 0 }, { 0, 0, 0, 1 } }; kalmanFilter.transitionMatrix = cv::Mat(4, 4, CV_32F, fltTransitionMatrixValues); // set Transition Matrix float fltMeasurementMatrixValues[2][4] = { { 1, 0, 0, 0 }, // declare an array of floats to feed into Kalman Filter Measurement Matrix, also known as Measurement Model { 0, 1, 0, 0 } }; kalmanFilter.measurementMatrix = cv::Mat(2, 4, CV_32F, fltMeasurementMatrixValues); // set Measurement Matrix cv::setIdentity(kalmanFilter.processNoiseCov, cv::Scalar::all(0.0001)); // default is 1, for smoothing try 0.0001 cv::setIdentity(kalmanFilter.measurementNoiseCov, cv::Scalar::all(10)); // default is 1, for smoothing try 10 cv::setIdentity(kalmanFilter.errorCovPost, cv::Scalar::all(0.1)); // default is 0, for smoothing try 0.1 cv::Mat imgBlank(700, 900, CV_8UC3, cv::Scalar::all(0)); // declare a blank image for moving the mouse over std::vector<cv::Point> predictedMousePositions; // declare 3 vectors for predicted, actual, and corrected positions std::vector<cv::Point> actualMousePositions; std::vector<cv::Point> correctedMousePositions; cv::namedWindow("imgBlank"); // declare window cv::setMouseCallback("imgBlank", mouseMoveCallback); // while (true) { cv::Mat matPredicted = kalmanFilter.predict(); cv::Point ptPredicted((int)matPredicted.at<float>(0), (int)matPredicted.at<float>(1)); cv::Mat matActualMousePosition(2, 1, CV_32F, cv::Scalar::all(0)); matActualMousePosition.at<float>(0, 0) = (float)ptActualMousePosition.x; matActualMousePosition.at<float>(1, 0) = (float)ptActualMousePosition.y; cv::Mat matCorrected = kalmanFilter.correct(matActualMousePosition); // function correct() updates the predicted state from the measurement cv::Point ptCorrected((int)matCorrected.at<float>(0), (int)matCorrected.at<float>(1)); predictedMousePositions.push_back(ptPredicted); actualMousePositions.push_back(ptActualMousePosition); correctedMousePositions.push_back(ptCorrected); // predicted, actual, and corrected are all now calculated, time to draw stuff drawCross(imgBlank, ptPredicted, SCALAR_BLUE); // draw a cross at the most recent predicted, actual, and corrected positions drawCross(imgBlank, ptActualMousePosition, SCALAR_WHITE); drawCross(imgBlank, ptCorrected, SCALAR_GREEN); for (int i = 0; i < predictedMousePositions.size() - 1; i++) { // draw each predicted point in blue cv::line(imgBlank, predictedMousePositions[i], predictedMousePositions[i + 1], SCALAR_BLUE, 1); } for (int i = 0; i < actualMousePositions.size() - 1; i++) { // draw each actual point in white cv::line(imgBlank, actualMousePositions[i], actualMousePositions[i + 1], SCALAR_WHITE, 1); } for (int i = 0; i < correctedMousePositions.size() - 1; i++) { // draw each corrected point in green cv::line(imgBlank, correctedMousePositions[i], correctedMousePositions[i + 1], SCALAR_GREEN, 1); } cv::imshow("imgBlank", imgBlank); // show the image cv::waitKey(10); // pause for a moment to get operating system to redraw the imgBlank imgBlank = cv::Scalar::all(0); // blank the imgBlank for next time around } return 0; }