Covariance class for QSET

covariance.cpp

//Covariance achieved with classes

#include <math.h> //Needed for pow() function

class Covariance {
        static int cycles = 5; //Should change based on frequency of data collection
        double sum1 = 0, sum2 = 0, arr1[cycles], arr2[cycles], var1 = 1, var2 = 1; //Initialize
        int counter = 0;

        public:
        void addData (double,double); //Use this to save data from each cycle
        double getData1() {return var1;} //Use this when publishing covariance data
        double getData2() {return var2;}
};

void Covariance::addData (double x, double y) {
        arr1[counter] = x; //Save data
        arr2[counter] = y;
        sum1 += x;
        sum2 += y;

        if (counter == cycles) { //Only update covariance when sufficient data is collected
                int temp = 0;
                for (int i = 0; i < cycles; i++) {
                        temp += pow(arr1[i]-(sum1/counter), 2); //Based on the variance equation
                }
                var1 = temp/counter;

                temp = 0;
                for (i = 0; i < cycles; i++) {
                        temp += pow(arr1[i]-(sum1/counter), 2);
                }
                var2 = temp/counter;

                counter = 0;
        } else {
                counter++;
        }
}

//Example:
//
// Covariance pos;
// pos.addData(4, 5);
// odom.pose.covariance[0] = pos.getData1();
// odom.pose.covariance[35] = pos.getData2();