Method CreateLinearAcceleration

src/Tracker/Kalman.cpp:240–293

\brief TKalmanFilter::CreateLinearAcceleration \param xy0 \param xyv0

/// \param xyv0
///
void TKalmanFilter::CreateLinearAcceleration(Point_t xy0, Point_t xyv0)
{
    // 6 state variables, 2 measurements
    m_linearKalman.init(6, 2, 0, El_t);
    // Transition cv::Matrix
    const track_t dt = m_deltaTime;
    const track_t dt2 = 0.5f * m_deltaTime * m_deltaTime;
    m_linearKalman.transitionMatrix = (cv::Mat_<track_t>(6, 6) <<
                                       1, 0, dt, 0,  dt2, 0,
                                       0, 1, 0,  dt, 0,   dt2,
                                       0, 0, 1,  0,  dt,  0,
                                       0, 0, 0,  1,  0,   dt,
                                       0, 0, 0,  0,  1,   0,
                                       0, 0, 0,  0,  0,   1);

    // init...
    m_lastPointResult = xy0;
    m_linearKalman.statePre.at<track_t>(0) = xy0.x;  // x
    m_linearKalman.statePre.at<track_t>(1) = xy0.y;  // y
    m_linearKalman.statePre.at<track_t>(2) = xyv0.x; // vx
    m_linearKalman.statePre.at<track_t>(3) = xyv0.y; // vy
    m_linearKalman.statePre.at<track_t>(4) = 0;      // ax
    m_linearKalman.statePre.at<track_t>(5) = 0;      // ay

    m_linearKalman.statePost.at<track_t>(0) = xy0.x;
    m_linearKalman.statePost.at<track_t>(1) = xy0.y;
    m_linearKalman.statePost.at<track_t>(2) = xyv0.x;
    m_linearKalman.statePost.at<track_t>(3) = xyv0.y;
    m_linearKalman.statePost.at<track_t>(4) = 0;
    m_linearKalman.statePost.at<track_t>(5) = 0;

    cv::setIdentity(m_linearKalman.measurementMatrix);

    track_t n1 = pow(m_deltaTime, 4.f) / 4.f;
    track_t n2 = pow(m_deltaTime, 3.f) / 2.f;
    track_t n3 = pow(m_deltaTime, 2.f);
    m_linearKalman.processNoiseCov = (cv::Mat_<track_t>(6, 6) <<
                                      n1, 0,  n2, 0,  n2, 0,
                                      0,  n1, 0,  n2, 0,  n2,
                                      n2, 0,  n3, 0,  n3, 0,
                                      0,  n2, 0,  n3, 0,  n3,
                                      0,  0,  n2, 0,  n3, 0,
                                      0,  0,  0,  n2, 0,  n3);

    m_linearKalman.processNoiseCov *= m_accelNoiseMag;

    cv::setIdentity(m_linearKalman.measurementNoiseCov, cv::Scalar::all(0.1));

    cv::setIdentity(m_linearKalman.errorCovPost, cv::Scalar::all(.1));

    m_initialPoints.reserve(MIN_INIT_VALS);

    m_initialized = true;
}

///
/// \brief TKalmanFilter::CreateLinearAcceleration