Kalman filter for SR300

    dists = new float[3 * numPixels]; // Dists contains XYZ values. needs to be 3x the size of numPixels
    amps = new float[numPixels];
    //frame = cvCreateImage(cvSize(depth_width, depth_height), 8, 3); // Create the frame
    frame = cv::Mat(cv::Size(depth_width, depth_height), CV_8UC3);

    KF.init(6, 3, 0);
    KF.transitionMatrix = (cv::Mat_<float>(6, 6) << 1, 0, 0, 1, 0, 0,
        0, 1, 0, 0, 1, 0,
        0, 0, 1, 0, 0, 1,
        0, 0, 0, 1, 0, 0,
        0, 0, 0, 0, 1, 0,
        0, 0, 0, 0, 0, 1);
    measurement = cv::Mat_<float>::zeros(3, 1);

    //Initaite Kalman
    KF.statePre.setTo(0);
    setIdentity(KF.measurementMatrix);
    setIdentity(KF.processNoiseCov, cv::Scalar::all(.001)); // Adjust this for faster convergence - but higher noise
    setIdentity(KF.measurementNoiseCov, cv::Scalar::all(1e-1));
    setIdentity(KF.errorCovPost, cv::Scalar::all(.1));