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;
}