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:


31 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
  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