Kalman Filter Implementation (Tracking mouse position)

KalmanFilter::KalmanFilter(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F)
Parameters:
  • dynamParams – Dimensionality of the state.
  • measureParams – Dimensionality of the measurement.
  • controlParams – Dimensionality of the control vector.
  • type – Type of the created matrices that should be CV_32F or CV_64F.
const Mat& KalmanFilter::predict(const Mat& control=Mat()) 
   - Computes a predicted state
const Mat& KalmanFilter::correct(const Mat& measurement)
  Updates the predicted state from the measurement.

Example:

-----------
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/video/tracking.hpp"
#include <Windows.h>

#define drawCross( center, color, d )                                 \
line( img, Point( center.x - d, center.y - d ), Point( center.x + d, center.y + d ), color, 2, CV_AA, 0); \
line( img, Point( center.x + d, center.y - d ), Point( center.x - d, center.y + d ), color, 2, CV_AA, 0 )

using namespace cv;
using namespace std;
 
int main( )
{ 

KalmanFilter KF(4, 2, 0);
POINT mousePos;
GetCursorPos(&mousePos);

// intialization of KF...
KF.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0,   0,1,0,1,  0,0,1,0,  0,0,0,1);
Mat_<float> measurement(2,1); measurement.setTo(Scalar(0));

KF.statePre.at<float>(0) = mousePos.x;
KF.statePre.at<float>(1) = mousePos.y;
KF.statePre.at<float>(2) = 0;
KF.statePre.at<float>(3) = 0;
setIdentity(KF.measurementMatrix);
setIdentity(KF.processNoiseCov, Scalar::all(1e-4));
setIdentity(KF.measurementNoiseCov, Scalar::all(10));
setIdentity(KF.errorCovPost, Scalar::all(.1));
// Image to show mouse tracking
Mat img(600, 800, CV_8UC3);
vector<Point> mousev,kalmanv;
mousev.clear();
kalmanv.clear();

while(1)
{
 // First predict, to update the internal statePre variable
 Mat prediction = KF.predict();
 Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
             
 // Get mouse point
 GetCursorPos(&mousePos);
 measurement(0) = mousePos.x;
 measurement(1) = mousePos.y; 
 
 // The update phase 
 Mat estimated = KF.correct(measurement);

 Point statePt(estimated.at<float>(0),estimated.at<float>(1));
 Point measPt(measurement(0),measurement(1));
    // plot points
    imshow("mouse kalman", img);
    img = Scalar::all(0);

    mousev.push_back(measPt);
    kalmanv.push_back(statePt);
    drawCross( statePt, Scalar(255,255,255), 5 );
    drawCross( measPt, Scalar(0,0,255), 5 );

    for (int i = 0; i < mousev.size()-1; i++) 
     line(img, mousev[i], mousev[i+1], Scalar(255,255,0), 1);
    
    for (int i = 0; i < kalmanv.size()-1; i++) 
     line(img, kalmanv[i], kalmanv[i+1], Scalar(0,155,255), 1);
    
 waitKey(10);  
}
                                          
    return 0;
}
-----------

Result:


51 comments:

  1. Hello, can you please post the code for the GetCursorPos function?
    thanks

    ReplyDelete
    Replies
    1. Hi. Here i have assumed that you are a windows OS user. And "Windows.h" header includes GetCursorPos function.

      For details please refer to
      http://msdn.microsoft.com/en-us/library/windows/desktop/ms648390%28v=vs.85%29.aspx

      Delete
    2. If your not on windows you can use: setMouseCallback()

      Delete
  2. For me this is not a real Kalman because you use the ground truth as your measure. You should add some noise on your mousePos variable to simulate a true measure system and plot 3 graphs : ground truth (GetCursorPos), measure (GetCursorPos + noise) and kalman prediction.

    ReplyDelete
  3. hi, i finally made this work.

    Could you explain me please how the transition matrix works ?

    i saw another examples of this mouse-kalman program and its diferent in a few details

    ReplyDelete
    Replies
    1. it models the state transition, the 4 matrix rows mean x' = x + dx, y' = y + dy, dx' = dx, dy' = dy. the state itself has 4 elements (x, y, dx, dy).

      Delete
  4. Is it the actual Kalman filter implementation friend ?

    ReplyDelete
  5. 22: KF.statePre.at(0) = mousePos.x;
    23: KF.statePre.at(1) = mousePos.y;

    should actually be

    KF.statePost.at(0) = mousePos.x;
    KF.statePost.at(1) = mousePos.y;

    as Kalman.predict() calculates statePre = TransitionMatrix * statePost;

    ReplyDelete
    Replies
    1. Exactly. I confirm the answer of Ommega after many hours of debug and headaches.

      Delete
  6. Hi can any one send the kalman filters for character recognition printed and handwritten

    ReplyDelete
  7. hi i want to track the person in video using kalman filtering.please help me i have used the mog2 operation to dected the person so i have sucessfully detected but now i want to track the object so how i do please help me

    ReplyDelete
  8. افضل الخدمات التى تسعى دائما ثراء الخليج لتقديمها من خلال الاعتماد على كل ما هوه جيد فى عالم الخدمات المنزلية من خدمات تسليك المجارى والبالوعات من خلال الاعتماد على مواد لها القدرة على تفتيت وازابة اصعب الرواسب مهما كان حجمها او قوة صلابتها دون احداث اى ضرر بالمواسير وخطوط الصرف الصح والبيارات الى جانب الاعتماد على خدمات تنظيف تتمتع بالتجديد الدائم فى الماكينات التى تستخدمها شركتنا فى التنظيف من خلال افضل ماكينات التنظيف التى تظهر بالسوق العالمى والاعتماد على منظفات معترف بها من قبل العالم كما تم توفير كم راع من العامالة التى تتمتع بالخبرة والدقة الشديدة فى عمليات التنظيف ولا تنسى اننا نعتمد على افضل المبيدات الحشرية فى عمليات القضاء على الحشرات بافضل مواتير الرش لذلك نمنح اطول فترات الضمان نظرا لقدرتنا العالية على التخلص من كافة انواع الحشرات المنزلية فلا تتردد بالاتصال بنا من خلال
    شركة تسليك مجارى بالدمام
    شركة تسليك المجارى بالدمام
    شركة شفط بيارات بالدمام
    شركة تنظيف بالدمام
    شركة تنظيف منازل بالدمام
    شركة تنظيف شقق بالدمام
    شركة تنظيف منازل بالدمام
    شركة تنظيف شقق بالدمام
    شركة نظافة بالدمام
    افضل شركة تنظيف بالدمام
    شركة تنظيف بالدمام
    شركة تنظيف منازل بالدمام
    شركة مكافحة حشرات بالدمام
    شركة مكافحة الحشرات بالدمام
    شركة رش مبيدات بالدمام
    شركة مكافحة النمل الابيض بالدمام

    ReplyDelete
  9. https://alsabaak.com/%D8%B4%D8%B1%D9%83%D8%A9-%D9%86%D8%B8%D8%A7%D9%81%D8%A9-%D8%A8%D8%AC%D8%AF%D8%A9/ شركة نظافة بجدة
    https://alsabaak.com/%D8%B4%D8%B1%D9%83%D8%A9-%D9%86%D8%B8%D8%A7%D9%81%D8%A9-%D8%A8%D8%AC%D8%AF%D8%A9/ شركة تنظيف شقق بجدة
    https://alsabaak.com/%D8%B4%D8%B1%D9%83%D8%A9-%D9%86%D8%B8%D8%A7%D9%81%D8%A9-%D8%A8%D8%AC%D8%AF%D8%A9/ شركة تنظيف منازل بجدة

    ReplyDelete
  10. يوجد المزيد من حملات الصيانة التي تقوم بها صيانة جروندنج لتصليح الاعطال الموجودة لدى اجهزتكم مع وجود ضمان على جميع قطع الغيار المعتمدة لدينا.
    التواصل معنا عبر:
    http://www.maintenanceg.com/Grundig-Agent-Egypt.html

    ReplyDelete

  11. تقدم لكم مراكز النور فرصة الالتحاق بكلية الهندسة و دخول امتحانات معادلة كلية هندسة بكل سهولة لما تقدمة من كورسات باسعار مناسبة لك و فترات تناسبك ايضا.
    تواصل معنا عبر:
    http://www.equation-college-engineering.com

    ReplyDelete
  12. يمكنكم التواصل مع شركة سوقني حيث تعمل على التسويق الكتروني للشركات من خلال السوشيال ميديا و المنتديات و المدونات الالكترونية و ذلك لوصول المواقع لاعلى النتائج في محركات البحث
    التواصل عبر:
    http://www.swaqny.com

    ReplyDelete
  13. اذا كنت تبحث عن التميز يمكنك التعامل مع اكبر توكيل صيانة سامسونج بمصر وعلي مستوي الجمهورية في جميع الاجهزه الكهربائية بأقل الاسعار واحدث قطع الغيار.
    للتواصل عبر البريد الالكتروني :
    http://www.maintenanceg.com/Samsung-Center-Agent-Egypt.html


    ReplyDelete
  14. يمكنك التواصل مع شركة الاخلاص والامانه اكبر الشركات بالمملكة العربية السعودية و نقل عفش الطائف
    بأحدث الاساليب والسيارات تحت اشراف مجموعه من امهر العمال بالمملكة العربية السعودية .
    للتواصل عبر الهاتف :
    0550018083
    او التواصل عبر البريد الالكتروني:
    http://www.xn-----jtd6bya2cendpd.com/%D9%86%D9%82%D9%84-%D8%B9%D9%81%D8%B4-%D8%A7%D9%84%D8%B7%D8%A7%D8%A6%D9%81-2


    ReplyDelete
  15. تواصل مع مؤسسة ابو خالد واختار من بين ديكورات مشبات
    مصنوعه من اجود الخامات و يمكنك الحصول علي ديكور راقي لمنزلك والوصول للدفء بمناسبة بداية فصل الشتاء .
    للتواصل عبر الهاتف:
    0530824463
    او عبر البريد الالكتروني:
    http://www.heateers.com


    ReplyDelete
  16. توفير افضل اساليب النقل من سيارات خاصه واوناش ضخمه لاستيعاب اكبر عدد ممكن من قطع الاثاث واثقلهم و نقل اثاث بالمدينه المنورة مع شركة فرسان الخليج اكبر الشركات بالمملكة العربية السعودية .
    للتواصل عبر الهاتف:
    0553061333
    او عبر البريد الالكتروني:
    http://www.forsan-elkhaleg.com/20/Moving-furniture-company-in-Madinah


    ReplyDelete
  17. يقوم فريق عمل شركة سوقني بأستخدام احدث برامج في تصميم مواقع الكترونية بأحدث الاشكال والالوان مع شركة سوقني اكبر الشركات بمصر وعلي مستوي الجمهورية .
    للتواصل عبر الهاتف
    00201008745590
    او عبر البريد الالكتروني:
    http://www.swaqny.com/Web-Design


    ReplyDelete
  18. Oh my god, what a beautiful article ! And I want to ask you 1 question : why state of kalman is 4 ?, But in source code you only use 2 elements : state[0] and state[1] ? Thank you very much.

    ReplyDelete
    Replies
    1. the other two elements model velocity (dx and dy). see also the transition matrix, it basically says x' = x + dx and y' = y + dy ("1,0,1,0, 0,1,0,1").

      Delete
  19. here's a version that only uses opencv gui functions, making it work on linux:


    #include "opencv2/highgui/highgui.hpp"
    #include "opencv2/video/tracking.hpp"

    #define drawCross( center, color, d ) \
    line( img, Point( center.x - d, center.y - d ), Point( center.x + d, center.y + d ), color, 2, CV_AA, 0); \
    line( img, Point( center.x + d, center.y - d ), Point( center.x - d, center.y + d ), color, 2, CV_AA, 0 )

    using namespace cv;
    using namespace std;

    Point mousePos;

    void mouseCallback(int event, int x, int y, int flags, void* userdata) {
    if ( event == EVENT_MOUSEMOVE ) {
    mousePos.x = x;
    mousePos.y = y;
    }
    }

    int main( )
    {

    KalmanFilter KF(4, 2, 0);

    // intialization of KF...
    KF.transitionMatrix = *(Mat_(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1);
    Mat_ measurement(2,1); measurement.setTo(Scalar(0));

    KF.statePre.at(0) = mousePos.x;
    KF.statePre.at(1) = mousePos.y;
    KF.statePre.at(2) = 0;
    KF.statePre.at(3) = 0;
    setIdentity(KF.measurementMatrix);
    setIdentity(KF.processNoiseCov, Scalar::all(1e-4));
    setIdentity(KF.measurementNoiseCov, Scalar::all(10));
    setIdentity(KF.errorCovPost, Scalar::all(.1));
    // Image to show mouse tracking
    Mat img(600, 800, CV_8UC3);
    vector mousev,kalmanv;
    mousev.clear();
    kalmanv.clear();

    namedWindow("mouse kalman", 1);
    setMouseCallback("mouse kalman", mouseCallback, NULL);

    while(1)
    {
    // First predict, to update the internal statePre variable
    Mat prediction = KF.predict();
    Point predictPt(prediction.at(0),prediction.at(1));

    // Get mouse point
    measurement(0) = mousePos.x;
    measurement(1) = mousePos.y;

    // The update phase
    Mat estimated = KF.correct(measurement);

    Point statePt(estimated.at(0),estimated.at(1));
    Point measPt(measurement(0),measurement(1));
    // plot points
    imshow("mouse kalman", img);
    img = Scalar::all(0);

    mousev.push_back(measPt);
    kalmanv.push_back(statePt);
    drawCross( statePt, Scalar(255,255,255), 5 );
    drawCross( measPt, Scalar(0,0,255), 5 );

    for (int i = 0; i < mousev.size()-1; i++)
    line(img, mousev[i], mousev[i+1], Scalar(255,255,0), 1);

    for (int i = 0; i < kalmanv.size()-1; i++)
    line(img, kalmanv[i], kalmanv[i+1], Scalar(0,155,255), 1);

    waitKey(10);
    }

    return 0;
    }

    ReplyDelete
  20. This comment has been removed by the author.

    ReplyDelete
  21. Hi Can anybody tell me why on line 20: KF.transitionMatrix = *(Mat_(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);


    no operator "*" matches these operands
    illegal indirection

    ReplyDelete
  22. Just remove the *... it worked foe me..

    ReplyDelete
  23. ضمان النجاح والتفوق فى امتحانات معادلة كلية الهندسة يضمنها لكم مركز النور مع افضل مجموعة من المتخصصين فى تبسيط وشرح المواد لضمان انجاح والتفوق
    فقط زورو موقعنا :


    ReplyDelete
  24. بادر بالتعرف علي شركة تنظيف شقق بالمدينة المنورة والتي توفر اكبر الخصومات علي خدمة تنظيف شقق بالمدينة المنورة باعلي جودة واقل الاسعار

    ReplyDelete
  25. سارع بالتعرف علي المزيد عن مركز صيانة فريجيدير لصيانة الاجهزة المعطلة

    ReplyDelete
  26. الان مع توكيل صيانة كاريير اكبر توكيلات الصيانة في مصر

    ReplyDelete
  27. تمتع بخدمات متميزه من اقوي شركة الان بالمملكة العربية السعودية شركة العنود التي تعتبر افضل شركة تنظيف خزانات بمكة لانها تقوم بتوفير بعض المواد والادوات الخاصه بها التي تعمل بها بعد تجربة واختبار من مجموعه من اهم الكوادر والخبراء المتميزون حيث انهم يقوموا بأعمال تنظيف خزانات بمكة والقضاء علي كل ماهو يعمل علي تلوث المياة الموجوده بالخزان وبالتالي يتم عزل خزانات بمكة للحفاظ علي الخزان من الداخل والخارج ومن ثم الحفاظ علي صحة الانسان في استخدام مياة نظيفه .

    ReplyDelete
  28. يمكنك الان وبكل سهولة التواصل مع صيانه يونيفرسال اذا كنت تعاني من اعطال بالاجهزه الكهربائةي المنزلية حيث الان يمكنك التخلص من جميع الاعطال بشكل نهائي تحت اشراف مجموعه من الخبراء ومن مهندسيين مدربين في توكيل يونيفرسال الذي يوفر افضل الطرق واجود الانواع من قطع الغيار الاصلية التي يمكن استبدالها بالقطع التالفه وباقل الاسعار .

    ReplyDelete
  29. اذا كنت تريد التواصل مع اكبر شركة انتاج وتركيب جميع انواع الاجهزه الكهربائية المنزلية من شركة يونيون اير يمكنك الان بكل سهولة الحصول علي افضل الخدمات مع فريق عمل متخصص يعنل بكل كفائه وجهد من اجل تحقيق اعلي نسبة في خدمة ورضاء العملاء حيث يوفر توكيل يونيون اير افضل فريق عمل متميز لخدمة اكبر عدد من العملاء ويمكنك التعرف علي الخدمات المقدمه اكثر عند التواصل معنا .

    ReplyDelete
  30. تعلن شركة الحارس الخاص عن توفير مجموعه من فرص وظائف مشرفين امن علي ان يتوفر لديهم بعض الخصائص والمميزات العقلية والجسمانية التي تؤهلهم للتعامل مع الاخطار وتأمين الاشخاص الهامه وحمايتهم من الاخطار وتأمين المنازل والاماكن العامه في وظائف حراس امن لحماية وتامين الممتلكات الخاصه يوميا وعلي مدار ال 24 ساعه تحت اشراف مجموعه من خبراء في وظائف افراد امن اليوم لديهم الخلفية الامنية في استخدام احدث الاساليب الامنية لسلامة العملاء .

    http://www.guards-security.com/jop/security-member/

    ReplyDelete