This snippet shows tracking mouse cursor with Python code from scratch and comparing the result with OpenCV. The CSV file that has been used are being created with below c++ code. A sample could be downloaded from here 1, 2, 3.

Python Kalman Filter

import numpy as np
np.set_printoptions(threshold=3)
np.set_printoptions(suppress=True)
from numpy import genfromtxt


#Notation used coming from: https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/


def prediction(X_hat_t_1,P_t_1,F_t,B_t,U_t,Q_t):
    X_hat_t=F_t.dot(X_hat_t_1)+(B_t.dot(U_t).reshape(B_t.shape[0],-1) )
    P_t=np.diag(np.diag(F_t.dot(P_t_1).dot(F_t.transpose())))+Q_t
    return X_hat_t,P_t
    

def update(X_hat_t,P_t,Z_t,R_t,H_t):
    
    K_prime=P_t.dot(H_t.transpose()).dot( np.linalg.inv ( H_t.dot(P_t).dot(H_t.transpose()) +R_t ) )  
    print("K:\n",K_prime)
    
    X_t=X_hat_t+K_prime.dot(Z_t-H_t.dot(X_hat_t))
    P_t=P_t-K_prime.dot(H_t).dot(P_t)
    
    return X_t,P_t


acceleration=0
delta_t=1/20#milisecond

groundTruth = genfromtxt('data/groundTruth.csv', delimiter=',',skip_header=1)

#Observations: position_X, position_Y
measurmens = genfromtxt('data/measurmens.csv', delimiter=',',skip_header=1)


#Checking our result with OpenCV
opencvKalmanOutput = genfromtxt('data/kalmanv.csv', delimiter=',',skip_header=1)

#Transition matrix
F_t=np.array([ [1 ,0,delta_t,0] , [0,1,0,delta_t] , [0,0,1,0] , [0,0,0,1] ])

#Initial State cov
P_t= np.identity(4)*0.2

#Process cov
Q_t= np.identity(4)

#Control matrix
B_t=np.array( [ [0] , [0], [0] , [0] ])

#Control vector
U_t=acceleration

#Measurment Matrix
H_t = np.array([ [1, 0, 0, 0], [ 0, 1, 0, 0]])

#Measurment cov
R_t= np.identity(2)*5

# Initial State
X_hat_t = np.array( [[0],[0],[0],[0]] )
print("X_hat_t",X_hat_t.shape)
print("P_t",P_t.shape)
print("F_t",F_t.shape)
print("B_t",B_t.shape)
print("Q_t",Q_t.shape)
print("R_t",R_t.shape)
print("H_t",H_t.shape)

for i in range(measurmens.shape[0]):
    X_hat_t,P_hat_t = prediction(X_hat_t,P_t,F_t,B_t,U_t,Q_t)
    print("Prediction:")
    print("X_hat_t:\n",X_hat_t,"\nP_t:\n",P_t)
    
    Z_t=measurmens[i].transpose()
    Z_t=Z_t.reshape(Z_t.shape[0],-1)
    
    print(Z_t.shape)
    
    X_t,P_t=update(X_hat_t,P_hat_t,Z_t,R_t,H_t)
    print("Update:")
    print("X_t:\n",X_t,"\nP_t:\n",P_t)
    X_hat_t=X_t
    P_hat_t=P_t
    
    print("=========================================")
    print("Opencv Kalman Output:")
    print("X_t:\n",opencvKalmanOutput[i])

import numpy as np

np.set_printoptions(threshold=3)

np.set_printoptions(suppress=True)

from numpy import genfromtxt

#Notation used coming from: https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/

def prediction(X_hat_t_1,P_t_1,F_t,B_t,U_t,Q_t):

X_hat_t=F_t.dot(X_hat_t_1)+(B_t.dot(U_t).reshape(B_t.shape[0],-1) )

P_t=np.diag(np.diag(F_t.dot(P_t_1).dot(F_t.transpose())))+Q_t

return X_hat_t,P_t

def update(X_hat_t,P_t,Z_t,R_t,H_t):

K_prime=P_t.dot(H_t.transpose()).dot( np.linalg.inv ( H_t.dot(P_t).dot(H_t.transpose()) +R_t ) )

print("K:\n",K_prime)

X_t=X_hat_t+K_prime.dot(Z_t-H_t.dot(X_hat_t))

P_t=P_t-K_prime.dot(H_t).dot(P_t)

return X_t,P_t

acceleration=0

delta_t=1/20#milisecond

groundTruth = genfromtxt('data/groundTruth.csv', delimiter=',',skip_header=1)

#Observations: position_X, position_Y

measurmens = genfromtxt('data/measurmens.csv', delimiter=',',skip_header=1)

#Checking our result with OpenCV

opencvKalmanOutput = genfromtxt('data/kalmanv.csv', delimiter=',',skip_header=1)

#Transition matrix

F_t=np.array([ [1 ,0,delta_t,0] , [0,1,0,delta_t] , [0,0,1,0] , [0,0,0,1] ])

#Initial State cov

P_t= np.identity(4)*0.2

#Process cov

Q_t= np.identity(4)

#Control matrix

B_t=np.array( [ [0] , [0], [0] , [0] ])

#Control vector

U_t=acceleration

#Measurment Matrix

H_t = np.array([ [1, 0, 0, 0], [ 0, 1, 0, 0]])

#Measurment cov

R_t= np.identity(2)*5

# Initial State

X_hat_t = np.array( [[0],[0],[0],[0]] )

print("X_hat_t",X_hat_t.shape)

print("P_t",P_t.shape)

print("F_t",F_t.shape)

print("B_t",B_t.shape)

print("Q_t",Q_t.shape)

print("R_t",R_t.shape)

print("H_t",H_t.shape)

for i in range(measurmens.shape[0]):

X_hat_t,P_hat_t = prediction(X_hat_t,P_t,F_t,B_t,U_t,Q_t)

print("Prediction:")

print("X_hat_t:\n",X_hat_t,"\nP_t:\n",P_t)

Z_t=measurmens[i].transpose()

Z_t=Z_t.reshape(Z_t.shape[0],-1)

print(Z_t.shape)

X_t,P_t=update(X_hat_t,P_hat_t,Z_t,R_t,H_t)

print("Update:")

print("X_t:\n",X_t,"\nP_t:\n",P_t)

X_hat_t=X_t

P_hat_t=P_t

print("=========================================")

print("Opencv Kalman Output:")

print("X_t:\n",opencvKalmanOutput[i])

C++ and OpenCV Kalman Filter

Rapidcsv has been downloaded from here

//http://www.morethantechnical.com/2011/06/17/simple-kalman-filter-for-tracking-using-opencv-2-2-w-code/
#include <iostream>
#include <vector>
#include <random>
#include <ctime>
#include <chrono>
#include <iomanip>

#include <Eigen/Dense>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>

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

namespace Eigen
{
    namespace internal
    {
        template<typename Scalar>
        struct scalar_normal_dist_op
        {
            static boost::mt19937 rng;    // The uniform pseudo-random algorithm
            mutable boost::normal_distribution<Scalar> norm;  // The gaussian combinator

            EIGEN_EMPTY_STRUCT_CTOR(scalar_normal_dist_op)

            template<typename Index>
            inline const Scalar operator() (Index, Index = 0) const { return norm(rng); }
        };

        template<typename Scalar> boost::mt19937 scalar_normal_dist_op<Scalar>::rng;

        template<typename Scalar>
        struct functor_traits<scalar_normal_dist_op<Scalar> >
        { enum
            { Cost = 50 * NumTraits<Scalar>::MulCost, PacketAccess = false, IsRepeatable = false };
        };
    }
}

template<typename Clock, typename Duration>
std::ostream &operator<<(std::ostream &stream,  const std::chrono::time_point<Clock, Duration> &time_point)
{
    const time_t time = Clock::to_time_t(time_point);
    struct tm tm;
    localtime_r(&time, &tm);
    return stream << std::put_time(&tm, "%c"); // Print standard date&time

}

auto start = std::chrono::high_resolution_clock::now();
int k=5;

Eigen::MatrixXd multivariateNormalDistribution()
{
    int size = 2; // Dimensionality (rows)
    int nn=1;     // How many samples (columns) to draw
    Eigen::internal::scalar_normal_dist_op<double> randN; // Gaussian functor
    //Eigen::internal::scalar_normal_dist_op<double>::rng.seed(1);
    auto elapsed = std::chrono::high_resolution_clock::now() - start;
    auto microseconds = std::chrono::duration_cast<std::chrono::microseconds>(elapsed).count();
    //std::cout<<"microseconds:" <<microseconds <<std::endl;
    Eigen::internal::scalar_normal_dist_op<double>::rng.seed(microseconds);
    Eigen::VectorXd mean(size);
    Eigen::MatrixXd covar(size,size);

    mean  <<  0,  0;
    covar <<  k*1e-0, 0,
       0,  k*1e-0;

    Eigen::MatrixXd normTransform(size,size);
    Eigen::LLT<Eigen::MatrixXd> cholSolver(covar);

    // We can only use the cholesky decomposition if
    // the covariance matrix is symmetric, pos-definite.
    // But a covariance matrix might be pos-semi-definite.
    // In that case, we'll go to an EigenSolver
    if (cholSolver.info()==Eigen::Success)
    {
    // Use cholesky solver
        normTransform = cholSolver.matrixL();
    }
    else
    {
        // Use eigen solver
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver(covar);
        normTransform = eigenSolver.eigenvectors() * eigenSolver.eigenvalues().cwiseSqrt().asDiagonal();
    }

    Eigen::MatrixXd samples = (normTransform * Eigen::MatrixXd::NullaryExpr(size,nn,randN)).colwise() + mean;
//    std::cout << "Mean\n" << mean << std::endl;
//    std::cout << "Covar\n" << covar << std::endl;
//    std::cout << "Samples\n" << samples << std::endl;
    return samples;
}

struct mouse_info_struct { int x,y; };
struct mouse_info_struct mouse_info = {-1,-1}, last_mouse;

std::vector<cv::Point> groundTruth,kalmanv,measurmens;

void on_mouse(int event, int x, int y, int flags, void* param) {
	{
		last_mouse = mouse_info;
		mouse_info.x = x;
		mouse_info.y = y;
	}
}

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


int main (int argc, char * const argv[]) {
    cv::Mat img(500, 500, CV_8UC3);
    cv::KalmanFilter KF(4, 2, 0);
    cv::Mat_<float> state(4, 1); /* (x, y, Vx, Vy) */
    cv::Mat processNoise(4, 1, CV_32F);
    cv::Mat_<float> measurement(2,1); measurement.setTo(cv::Scalar(0));
    char code = char(-1);
	
    cv::namedWindow("Mouse Tracking with Kalman Filter");
    cv::setMouseCallback("Mouse Tracking with Kalman Filter", on_mouse, nullptr);
    double delta_t=1/20.0;
	
    for(;;)
    {
		if (mouse_info.x < 0 || mouse_info.y < 0) {
            imshow("Mouse Tracking with Kalman Filter", img);
            cv::waitKey(30);
			continue;
		}
        cv::Mat transitionMatrix=(cv::Mat_<float>(4, 4) << 1,0,delta_t,0,   0,1,0,delta_t,  0,0,1,0,  0,0,0,1);
        KF.transitionMatrix = transitionMatrix;
		
        setIdentity(KF.measurementMatrix);
        cv::setIdentity(KF.processNoiseCov, cv::Scalar::all(1e-0));
        cv::setIdentity(KF.measurementNoiseCov, cv::Scalar::all(k*1e-0));
        cv::setIdentity(KF.errorCovPost, cv::Scalar::all(.2));
        cv::setIdentity(KF.errorCovPre,cv::Scalar::all(.1));

        measurmens.clear();
        groundTruth.clear();
		kalmanv.clear();
        std::cout<< "measurementMatrix"<<std::endl;
        std::cout<<KF.measurementMatrix<<std::endl;
		
        for(;;)
        {
            std::cout<< "processNoiseCov"<<std::endl;
            std::cout<< KF.processNoiseCov<<std::endl;

            std::cout<< "measurementNoiseCov"<<std::endl;
            std::cout<< KF.measurementNoiseCov<<std::endl;

            std::cout<<"State Prior (before calling predict function):" <<std::endl;
            std::cout<<KF.statePre <<std::endl;

            std::cout<<"Cov Prior (before calling predict function):" <<std::endl;
            std::cout<<KF.errorCovPre <<std::endl;

            std::cout<<"Cov Posterior (before calling predict function):" <<std::endl;
            std::cout<<KF.errorCovPost <<std::endl;

            //KF.controlMatrix
            std::cout<<"My State Prediction:" <<std::endl;
            std::cout<<KF.transitionMatrix*KF.statePost<<std::endl;
            std::cout<<"My Cov Prediction:" <<std::endl;
            std::cout<<KF.transitionMatrix*KF.errorCovPost*KF.transitionMatrix.t()+KF.processNoiseCov<<std::endl;

            cv::Mat prediction = KF.predict();
            cv::Point predictPt(prediction.at<float>(0),prediction.at<float>(1));

            std::cout<<"OpenCV Prediction:" <<std::endl;

            std::cout<<"State Prior:" <<std::endl;
            std::cout<<KF.statePre <<std::endl;

            std::cout<<"OpenCV Cov Prior:" <<std::endl;
            std::cout<<KF.errorCovPre <<std::endl;


            measurement(0) = mouse_info.x+multivariateNormalDistribution()(0,0);
            measurement(1) = mouse_info.y+multivariateNormalDistribution()(1,0);

            cv::Point groundtruth(mouse_info.x,mouse_info.y);
            groundTruth.push_back(groundtruth);


            std::cout<<"Ground Truth:" <<std::endl;
            std::cout<< mouse_info.x<<" , "<<mouse_info.y <<std::endl;

            cv::Point measPt(measurement(0),measurement(1));
            measurmens.push_back(measPt);


            cv::Mat estimated = KF.correct(measurement);
            cv::Point statePt(estimated.at<float>(0),estimated.at<float>(1));
			kalmanv.push_back(statePt);

            std::cout<<"My State Posterior:" <<std::endl;
            std::cout<<KF.statePre+KF.gain*(measurement-KF.measurementMatrix*KF.statePre) <<std::endl;

            std::cout<<"My Cov Posterior:" <<std::endl;
            std::cout<<(cv::Mat::eye(4,4, CV_32F) - KF.gain*KF.measurementMatrix)*KF.errorCovPre <<std::endl;

            std::cout<<"Opencv State Posterior:" <<std::endl;
            std::cout<<KF.statePost <<std::endl;

            std::cout<<"Opencv Cov Posterior:" <<std::endl;
            std::cout<<KF.errorCovPost <<std::endl;

            std::cout<<"-----------------------------------------------" <<std::endl;
            multivariateNormalDistribution();

            img = cv::Scalar::all(0);
            drawCross( statePt, cv::Scalar(255,255,255), 5 );
            drawCross( measPt, cv::Scalar(0,0,255), 5 );
            for (std::size_t i = 0; i < groundTruth.size()-1; i++)
            {
                line(img, groundTruth[i], groundTruth[i+1], cv::Scalar(0,255,0), 1);
			}
            for (std::size_t i = 0; i < kalmanv.size()-1; i++)
            {
                line(img, kalmanv[i], kalmanv[i+1], cv::Scalar(255,0,0), 1);
			}
            for (std::size_t i = 0; i < measurmens.size()-1; i++)
            {
                line(img, measurmens[i], measurmens[i+1], cv::Scalar(0,255,255), 1);
            }
            cv::putText(img,"Noisy Measurements",cv::Point(10,10),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(0,255,255) 	);
            cv::putText(img,"Real Mouse Position(ground truth)",cv::Point(10,25),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(0,255,0));
            cv::putText(img,"Kalman Sate",cv::Point(10,35),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(255,0,0));


            imshow( "Mouse Tracking with Kalman Filter", img );
            code = char(cv::waitKey(1000.0*delta_t));
			
            if( code > 0 )
                break;
        }
        if( code == 27 || code == 'q' || code == 'Q' )
            break;
    }


    std::ofstream groundTruthfile("groundTruth.csv",std::ofstream::ate );
    groundTruthfile<<"x,y"<<std::endl;
    for (std::size_t i = 0; i < groundTruth.size(); i++)
    {
        groundTruthfile<<groundTruth[i].x << ","<<groundTruth[i].y <<std::endl;
    }
    groundTruthfile.close();




    std::ofstream kalmanvfile("kalmanv.csv",std::ofstream::ate );
    kalmanvfile<<"x,y"<<std::endl;
    for (std::size_t i = 0; i < kalmanv.size()-1; i++)
    {
        kalmanvfile<<kalmanv[i].x << ","<<kalmanv[i].y <<std::endl;
    }
    kalmanvfile.close();

    std::ofstream measurmensfile("measurmens.csv",std::ofstream::ate );
    measurmensfile<<"x,y"<<std::endl;
    for (std::size_t i = 0; i < kalmanv.size()-1; i++)
    {
        measurmensfile<<measurmens[i].x << ","<<measurmens[i].y <<std::endl;
    }
    measurmensfile.close();

    std::cout<<"~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" <<std::endl;

    return 0;
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

//http://www.morethantechnical.com/2011/06/17/simple-kalman-filter-for-tracking-using-opencv-2-2-w-code/

#include <iostream>

#include <vector>

#include <random>

#include <ctime>

#include <chrono>

#include <iomanip>

#include <Eigen/Dense>

#include <boost/random/mersenne_twister.hpp>

#include <boost/random/normal_distribution.hpp>

#include <opencv2/highgui/highgui.hpp>

#include <opencv2/video/tracking.hpp>

#include "rapidcsv.h"

namespace Eigen

{

namespace internal

{

template<typename Scalar>

struct scalar_normal_dist_op

{

static boost::mt19937 rng; // The uniform pseudo-random algorithm

mutable boost::normal_distribution<Scalar> norm; // The gaussian combinator

EIGEN_EMPTY_STRUCT_CTOR(scalar_normal_dist_op)

template<typename Index>

inline const Scalar operator() (Index, Index = 0) const { return norm(rng); }

};

template<typename Scalar> boost::mt19937 scalar_normal_dist_op<Scalar>::rng;

template<typename Scalar>

struct functor_traits<scalar_normal_dist_op<Scalar> >

{ enum

{ Cost = 50 * NumTraits<Scalar>::MulCost, PacketAccess = false, IsRepeatable = false };

};

}

template<typename Clock, typename Duration>

std::ostream &operator<<(std::ostream &stream, const std::chrono::time_point<Clock, Duration> &time_point)

{

const time_t time = Clock::to_time_t(time_point);

struct tm tm;

localtime_r(&time, &tm);

return stream << std::put_time(&tm, "%c"); // Print standard date&time

}

auto start = std::chrono::high_resolution_clock::now();

int k=5;

Eigen::MatrixXd multivariateNormalDistribution()

{

int size = 2; // Dimensionality (rows)

int nn=1; // How many samples (columns) to draw

Eigen::internal::scalar_normal_dist_op<double> randN; // Gaussian functor

//Eigen::internal::scalar_normal_dist_op<double>::rng.seed(1);

auto elapsed = std::chrono::high_resolution_clock::now() - start;

auto microseconds = std::chrono::duration_cast<std::chrono::microseconds>(elapsed).count();

//std::cout<<"microseconds:" <<microseconds <<std::endl;

Eigen::internal::scalar_normal_dist_op<double>::rng.seed(microseconds);

Eigen::VectorXd mean(size);

Eigen::MatrixXd covar(size,size);

mean << 0, 0;

covar << k*1e-0, 0,

0, k*1e-0;

Eigen::MatrixXd normTransform(size,size);

Eigen::LLT<Eigen::MatrixXd> cholSolver(covar);

// We can only use the cholesky decomposition if

// the covariance matrix is symmetric, pos-definite.

// But a covariance matrix might be pos-semi-definite.

// In that case, we'll go to an EigenSolver

if (cholSolver.info()==Eigen::Success)

{

// Use cholesky solver

normTransform = cholSolver.matrixL();

}

else

{

// Use eigen solver

Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver(covar);

normTransform = eigenSolver.eigenvectors() * eigenSolver.eigenvalues().cwiseSqrt().asDiagonal();

}

Eigen::MatrixXd samples = (normTransform * Eigen::MatrixXd::NullaryExpr(size,nn,randN)).colwise() + mean;

// std::cout << "Mean\n" << mean << std::endl;

// std::cout << "Covar\n" << covar << std::endl;

// std::cout << "Samples\n" << samples << std::endl;

return samples;

}

struct mouse_info_struct { int x,y; };

struct mouse_info_struct mouse_info = {-1,-1}, last_mouse;

std::vector<cv::Point> groundTruth,kalmanv,measurmens;

void on_mouse(int event, int x, int y, int flags, void* param) {

{

last_mouse = mouse_info;

mouse_info.x = x;

mouse_info.y = y;

}

// plot points

#define drawCross( center, color, d ) \

cv::line( img, cv::Point( center.x - d, center.y - d ), \

cv::Point( center.x + d, center.y + d ), color, 2, CV_AA, 0); \

cv::line( img, cv::Point( center.x + d, center.y - d ), \

cv::Point( center.x - d, center.y + d ), color, 2, CV_AA, 0 )

int main (int argc, char * const argv[]) {

cv::Mat img(500, 500, CV_8UC3);

cv::KalmanFilter KF(4, 2, 0);

cv::Mat_<float> state(4, 1); /* (x, y, Vx, Vy) */

cv::Mat processNoise(4, 1, CV_32F);

cv::Mat_<float> measurement(2,1); measurement.setTo(cv::Scalar(0));

char code = char(-1);

cv::namedWindow("Mouse Tracking with Kalman Filter");

cv::setMouseCallback("Mouse Tracking with Kalman Filter", on_mouse, nullptr);

double delta_t=1/20.0;

for(;;)

{

if (mouse_info.x < 0 || mouse_info.y < 0) {

imshow("Mouse Tracking with Kalman Filter", img);

cv::waitKey(30);

continue;

}

cv::Mat transitionMatrix=(cv::Mat_<float>(4, 4) << 1,0,delta_t,0, 0,1,0,delta_t, 0,0,1,0, 0,0,0,1);

KF.transitionMatrix = transitionMatrix;

setIdentity(KF.measurementMatrix);

cv::setIdentity(KF.processNoiseCov, cv::Scalar::all(1e-0));

cv::setIdentity(KF.measurementNoiseCov, cv::Scalar::all(k*1e-0));

cv::setIdentity(KF.errorCovPost, cv::Scalar::all(.2));

cv::setIdentity(KF.errorCovPre,cv::Scalar::all(.1));

measurmens.clear();

groundTruth.clear();

kalmanv.clear();

std::cout<< "measurementMatrix"<<std::endl;

std::cout<<KF.measurementMatrix<<std::endl;

for(;;)

{

std::cout<< "processNoiseCov"<<std::endl;

std::cout<< KF.processNoiseCov<<std::endl;

std::cout<< "measurementNoiseCov"<<std::endl;

std::cout<< KF.measurementNoiseCov<<std::endl;

std::cout<<"State Prior (before calling predict function):" <<std::endl;

std::cout<<KF.statePre <<std::endl;

std::cout<<"Cov Prior (before calling predict function):" <<std::endl;

std::cout<<KF.errorCovPre <<std::endl;

std::cout<<"Cov Posterior (before calling predict function):" <<std::endl;

std::cout<<KF.errorCovPost <<std::endl;

//KF.controlMatrix

std::cout<<"My State Prediction:" <<std::endl;

std::cout<<KF.transitionMatrix*KF.statePost<<std::endl;

std::cout<<"My Cov Prediction:" <<std::endl;

std::cout<<KF.transitionMatrix*KF.errorCovPost*KF.transitionMatrix.t()+KF.processNoiseCov<<std::endl;

cv::Mat prediction = KF.predict();

cv::Point predictPt(prediction.at<float>(0),prediction.at<float>(1));

std::cout<<"OpenCV Prediction:" <<std::endl;

std::cout<<"State Prior:" <<std::endl;

std::cout<<KF.statePre <<std::endl;

std::cout<<"OpenCV Cov Prior:" <<std::endl;

std::cout<<KF.errorCovPre <<std::endl;

measurement(0) = mouse_info.x+multivariateNormalDistribution()(0,0);

measurement(1) = mouse_info.y+multivariateNormalDistribution()(1,0);

cv::Point groundtruth(mouse_info.x,mouse_info.y);

groundTruth.push_back(groundtruth);

std::cout<<"Ground Truth:" <<std::endl;

std::cout<< mouse_info.x<<" , "<<mouse_info.y <<std::endl;

cv::Point measPt(measurement(0),measurement(1));

measurmens.push_back(measPt);

cv::Mat estimated = KF.correct(measurement);

cv::Point statePt(estimated.at<float>(0),estimated.at<float>(1));

kalmanv.push_back(statePt);

std::cout<<"My State Posterior:" <<std::endl;

std::cout<<KF.statePre+KF.gain*(measurement-KF.measurementMatrix*KF.statePre) <<std::endl;

std::cout<<"My Cov Posterior:" <<std::endl;

std::cout<<(cv::Mat::eye(4,4, CV_32F) - KF.gain*KF.measurementMatrix)*KF.errorCovPre <<std::endl;

std::cout<<"Opencv State Posterior:" <<std::endl;

std::cout<<KF.statePost <<std::endl;

std::cout<<"Opencv Cov Posterior:" <<std::endl;

std::cout<<KF.errorCovPost <<std::endl;

std::cout<<"-----------------------------------------------" <<std::endl;

multivariateNormalDistribution();

img = cv::Scalar::all(0);

drawCross( statePt, cv::Scalar(255,255,255), 5 );

drawCross( measPt, cv::Scalar(0,0,255), 5 );

for (std::size_t i = 0; i < groundTruth.size()-1; i++)

{

line(img, groundTruth[i], groundTruth[i+1], cv::Scalar(0,255,0), 1);

}

for (std::size_t i = 0; i < kalmanv.size()-1; i++)

{

line(img, kalmanv[i], kalmanv[i+1], cv::Scalar(255,0,0), 1);

}

for (std::size_t i = 0; i < measurmens.size()-1; i++)

{

line(img, measurmens[i], measurmens[i+1], cv::Scalar(0,255,255), 1);

}

cv::putText(img,"Noisy Measurements",cv::Point(10,10),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(0,255,255) );

cv::putText(img,"Real Mouse Position(ground truth)",cv::Point(10,25),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(0,255,0));

cv::putText(img,"Kalman Sate",cv::Point(10,35),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(255,0,0));

imshow( "Mouse Tracking with Kalman Filter", img );

code = char(cv::waitKey(1000.0*delta_t));

if( code > 0 )

break;

}

if( code == 27 || code == 'q' || code == 'Q' )

break;

}

std::ofstream groundTruthfile("groundTruth.csv",std::ofstream::ate );

groundTruthfile<<"x,y"<<std::endl;

for (std::size_t i = 0; i < groundTruth.size(); i++)

{

groundTruthfile<<groundTruth[i].x << ","<<groundTruth[i].y <<std::endl;

}

groundTruthfile.close();

std::ofstream kalmanvfile("kalmanv.csv",std::ofstream::ate );

kalmanvfile<<"x,y"<<std::endl;

for (std::size_t i = 0; i < kalmanv.size()-1; i++)

{

kalmanvfile<<kalmanv[i].x << ","<<kalmanv[i].y <<std::endl;

}

kalmanvfile.close();

std::ofstream measurmensfile("measurmens.csv",std::ofstream::ate );

measurmensfile<<"x,y"<<std::endl;

for (std::size_t i = 0; i < kalmanv.size()-1; i++)

{

measurmensfile<<measurmens[i].x << ","<<measurmens[i].y <<std::endl;

}

measurmensfile.close();

std::cout<<"~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" <<std::endl;

return 0;

}

4.4 7 votes

Article Rating

This site uses Akismet to reduce spam. Learn how your comment data is processed.

23 Comments

Oldest

Newest Most Voted

Inline Feedbacks

View all comments

john

5 years ago

Thanks for sharing.keep up the good work.

Author

admin

Reply to john

Thanks for your comments, I hope that have helped you.

djamila_st

good article

Ardian

Hi,

Thanks for the tutorial. I try the code and compare all the errors (RMSE) w.r.t the given ground truth. Here is the result:
1. Meassurement error: 1.145
2. opencv kalman error: 5.80
3. your code of kalman error: 7.92

Why do we have larger error on estimated kalman than the measurement? Thanks.

Hung

9 months ago

Reply to Ardian

i think the python code have some mistake. Because it think the control matrix B_t i think it is [deltaT^2][deltaT] but in this code is full zero. I don’t know why and can some one explain it for me.

Reply to Hung

If the system is under no external forces, we set the control matrix to zero, meaning no external force is changing the system

Thank you and thank you for sharing

Thank you so much for this information. It is very helpful.

I think this is one of the best blogs for me because this is really helpful for me.

noor

thanks for sharing

thanks for the reports

Jonathon Walker

4 years ago

I found this blog from Ben Nam’s youtube video, “https://www.youtube.com/watch?v=jn8vQSEGmuM,” and would like to use a lot of this code and ideas in my non-thesis research paper. I am guessing that you have all this up for individuals to use and I plan to thoroughly cite this website directly. If you have any specifics or real names you’d like me to cite or restrictions on use, please contact me and let me know.

Reply to Jonathon Walker

Hello Jonathon,
I’m glad that you found my website useful, the codes are under BSD license, which means you can do whatever you want them,
in the case you need to cite something or contact:
my linked in https://www.linkedin.com/in/realbehnam/
my GitHub: https://github.com/behnamasadi/

Cheers.

Thanks for the great reminders, I’m not finding that poll page.

vaidehi

how we can get real time sensor data into this program instead of csv file?

Reply to vaidehi

Hello, well you need driver for your sensor so you can read data from sensor and then just feed it one by one to update state in the algorithm. Regards.

ferahtia.FS

3 years ago

merci pour les informations

Luka

Thank You for the great explanation and the example code. I looked at the Python code and can understand it. But how can I apply this code to some data? I assume I need a csv-file with the groundTruth and measruements. But how do I obtain ground Truth? I only could imagine measuring something with some noise and collecting the data but I don’t have the ground truth. Also can you explain what kind of data “opencvKalmanOutput” contains? I thought we can calculate Kalman Gain based on our other values, so why do we need a dataset for it? Would… Read more »

Reply to Luka

Dear Luka, thanks for your comment, normally you only have measurements and prediction from your model, in my case I was moving around the mouse, so I had the exact position (I record the exact mouse position just for accuracy comparison) then I compared OpenCV kalman filter output with my own implementation to verify the correctness of my code. Hope that helps. Cheers.

fatmer

his is perTfect, thanks!
http://virtuelcampus.univ-msila.dz/facsh

Alsha

2 years ago

hai,
I tried your code to learn C++ programming, but I’m confused why the value in ‘measuremens’ is always an integer.

is std::vector<cv::Point> of type integer? i tried to push_back a float value to std::vector<cv::Point> measurmens, but when i print measurmens the value becomes integer.

so how to keep the value in measurmens of type float?

Reply to Alsha

Because everything is a pixel value in this code, cv::Point is a template class for 2D points specified by its coordinates x and y. Please check this page. typedef Point_ Point2f; is what you need, so for instance: Point2f a(0.3f, 0.f), b(0.f, 0.4f);

Alaa Nabih

1 year ago

hi Behnam,
could you please share a useful source for “Information Fusion using Kalman filter”
?
Best Regards,

Cookie	Duration	Description
cookielawinfo-checbox-analytics	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookie is used to store the user consent for the cookies in the category "Analytics".
cookielawinfo-checbox-functional	11 months	The cookie is set by GDPR cookie consent to record the user consent for the cookies in the category "Functional".
cookielawinfo-checbox-others	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookie is used to store the user consent for the cookies in the category "Other.
cookielawinfo-checkbox-necessary	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookies is used to store the user consent for the cookies in the category "Necessary".
cookielawinfo-checkbox-performance	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookie is used to store the user consent for the cookies in the category "Performance".
viewed_cookie_policy	11 months	The cookie is set by the GDPR Cookie Consent plugin and is used to store whether or not user has consented to the use of cookies. It does not store any personal data.