二维KalManFilter滤波原理及C/C++源码

二维KalManFilter滤波原理及C/C++源码

---

原理介绍

---

前言

在工作过程中，遇到关于KalManFilter的算法，因此，本文就二维KalManFilter进行原理简要的介绍，重要的是使用C语言实现其过程，并简单用于鼠标坐标实时监测。

---

一、KalManFillter原理简介

下面为KalmanFilter的推导公式，蓝色为预测部分：状态向量x’和状态协方差p’，绿色部分：计算Kalman增益K，黄色部分：更新系统协方差矩阵P和最终的预测值x。Q为过程噪声，R为测量噪声。

二、代码实现

1.矩阵操作函数

由于是2维KalManFilter，设计的矩阵计算，先实现需要用到的矩阵函数，头文件代码如下：

#pragma once
//申请最大矩阵内存
#define MATSIZE 50
//结构体
 struct Matrix
{
	int row,col;		//row为行,col
	Matrix() {};
	Matrix(int _row,int _col) :row(_row), col(_col) {}
	double data[MATSIZE];
};

void InitMatrix(Matrix *matrix, int col, int row,int value);		//初始化矩阵
void ValueMatrix(Matrix *matrix, double *array);				//给一个矩阵赋值
int SizeMatrix(Matrix *matrix);								//获得一个矩阵的大小
void CopyMatrix(Matrix *matrix_A, Matrix *matrix_B);		//复制一个矩阵的值
void PrintMatrix(Matrix *matrix);							//打印一个矩阵

															//矩阵的基本运算
void AddMatrix(Matrix matrix_A, Matrix matrix_B, Matrix *matrix_C);		//矩阵的加法
void SubMatrix(Matrix matrix_A, Matrix matrix_B, Matrix *matrix_C);		//矩阵剑法
void MulMatrix(Matrix matrix_A, Matrix matrix_B, Matrix *matrix_C);		//矩阵的乘法
void TransMatrix(Matrix matrix, Matrix *matrix_C);							//矩阵的转置
void InvMatrix(Matrix matrix_A, Matrix *matrix_C);                         //逆矩阵

然后，实现矩阵加减乘除，转置以及逆矩阵（2*2）的函数。代码如下：

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include"MatFun.h"

void InitMatrix(Matrix *matrix, int col, int row,int value)				//初始化一个矩阵
{
	if (col>0 && row>0)
	{
		matrix->col = col;
		matrix->row = row;
		memset(matrix->data, value, sizeof(double)*col*row);
		return ;
	}
	else
		return ;
}

void ValueMatrix(Matrix *matrix, double *array) 		//给矩阵赋值
{
	if (matrix->data != NULL)
	{
		memcpy(matrix->data, array, matrix->col*matrix->row * sizeof(double));
	}
}

int SizeMatrix(Matrix *matrix)
{
	return matrix->col*matrix->row;
}

void CopyMatrix(Matrix *matrix_A, Matrix *matrix_B)
{
	matrix_B->col = matrix_A->col;
	matrix_B->row = matrix_A->row;
	memcpy(matrix_B->data, matrix_A->data, SizeMatrix(matrix_A) * sizeof(double));
}

void PrintMatrix(Matrix *matrix)
{
	for (int i = 0; i<SizeMatrix(matrix); i++)
	{
		printf("%lf\t", matrix->data[i]);
		if ((i + 1) % matrix->row == 0)
			printf("\n");
	}

}
//加法
void AddMatrix(Matrix matrix_A, Matrix matrix_B, Matrix *matrix_C)
{

	if (matrix_A.col == matrix_B.col && matrix_A.row == matrix_B.row)
	{
		for (int i = 0; i<matrix_A.row; i++)
		{
			for (int j = 0; j<matrix_A.col; j++)
			{
				matrix_C->data[i*matrix_C->col + j] = \
					matrix_A.data[i*matrix_A.col + j] + matrix_B.data[i*matrix_A.col + j];
			}
		}
	}
	else
	{
		printf("不可相加\n");
	}
}


//减法
void SubMatrix(Matrix matrix_A, Matrix matrix_B, Matrix *matrix_C)
{
	matrix_C->row = matrix_A.row;
	matrix_C->col = matrix_A.col;
	if (matrix_A.col == matrix_B.col && matrix_A.row == matrix_B.row)
	{
		for (int i = 0; i<matrix_A.row; i++)
		{
			for (int j = 0; j<matrix_A.col; j++)
			{
				matrix_C->data[i*matrix_C->col + j] = \
					matrix_A.data[i*matrix_A.col + j] - matrix_B.data[i*matrix_A.col + j];
			}
		}
	}
	else
	{
		printf("不可相加\n");
	}
}


//乘法
void MulMatrix(Matrix matrix_A, Matrix matrix_B, Matrix *matrix_C)
{
	matrix_C->row = matrix_A.row;

	matrix_C->col = matrix_B.col;
	memset(matrix_C->data, 0, MATSIZE*sizeof(double));
	if (matrix_A.col == matrix_B.row)		//列==行
	{
		
		for (int i = 0; i<matrix_A.row; i++)
		{
			for (int j = 0; j<matrix_B.col; j++)
			{
				for (int k = 0; k<matrix_A.row; k++)
				{
					matrix_C->data[i*matrix_C->col + j] += \
						matrix_A.data[i*matrix_A.col + k] * matrix_B.data[k*matrix_B.col + j];
				}
			}
		}
	}
	else
	{
		printf("不可相乘\n");
	}
}




//inv
void InvMatrix(Matrix matrix_A, Matrix *matrix_C)
{
	matrix_C->col = matrix_A.col;
	matrix_C->row = matrix_A.row;

	double f,k;
	if (matrix_A.col==2&&matrix_A.row==2)		//列==行
	{
		f = matrix_A.data[3] * matrix_A.data[0] - matrix_A.data[1] * matrix_A.data[2];
		if (f == 0) { printf("不可逆\n"); return; }
		else
			k = 1 / f;
		matrix_C->data[0] = matrix_A.data[3] *k;
		matrix_C->data[1] = -matrix_A.data[1] *k;
		matrix_C->data[2] = -matrix_A.data[2] *k;
		matrix_C->data[3] = matrix_A.data[0] * k;
	}
	else
	{
		printf("不可逆\n");
		return ;
	}
}


//矩阵转置
void TransMatrix(Matrix matrix,Matrix *matrixTemp)			//条件为方阵
{
	matrixTemp->col = matrix.row;
	matrixTemp->row = matrix.col;

	if (matrix.col>0&& matrix.row>0)
	{

		for (int i = 0; i<matrix.col; i++)
		{
			for (int j = 0; j<matrix.row; j++)
			{
				matrixTemp->data[i*matrixTemp->col + j] = matrix.data[j*matrix.col + i];
			}
		}
		return ;
	}
	else
	{
		printf("转置的矩阵必须为方阵\n");
	}
}

2.KalManFilter实现函数

头文件：

#pragma once
#include"MatFun.h"

void KalManInit(double x, double y);
void KalManm_Predict(double X, double Y, double* _X, double* _Y);

cpp文件：

#include"MatFun.h"
#include"kalman.h"

//状态转移矩阵
double m_T[16] = {
	1, 0, 1, 0,
	0, 1, 0, 1,
	0, 0, 1, 0,
	0, 0, 0, 1 };

//状态矩阵
double m_H[8] = {
	1, 0, 0, 0,
	0, 1, 0, 0 };

//过程噪声
double m_Q[16] = {
	1, 0, 1, 0,
	0, 1, 0, 1,
	0, 0, 1, 0,
	0, 0, 0, 1 };

//状态协方差矩阵
double m_P[16] = {
	1, 0, 1, 0,
	0, 1, 0, 1,
	0, 0, 1, 0,
	0, 0, 0, 1 };

//测量噪声
double m_R[4] = {
	100000, 0,
	0, 100000 };

//单位矩阵
double m_I[16] = {
	1, 0, 1, 0,
	0, 1, 0, 1,
	0, 0, 1, 0,
	0, 0, 0, 1 };

Matrix xx(4, 1), yy(2, 1);
Matrix H(2, 4), Q(4, 4), P(4, 4), R(2, 2), I(4, 4);
Matrix T(4, 4);

//中间参数矩阵
Matrix Pre_x(4, 1);
Matrix Pre_y(2, 1);  

Matrix Pre_p(4, 4);     //后验协方差
Matrix x_final(2, 1);  //最终预测坐标

Matrix K(2, 4);       //kalman增益
Matrix S(2, 2);


//临时变量
Matrix tmp2, tmp1, tT, t_H, invS;

void KalManInit(double X, double Y) {

	//初始化矩阵
	//ValueMatrix(&m_x, x);
	//ValueMatrix(&m_yy, yy);
	ValueMatrix(&H, m_H);
	ValueMatrix(&Q, m_Q);
	ValueMatrix(&P, m_P);
	ValueMatrix(&R, m_R);
	ValueMatrix(&I, m_I);
	ValueMatrix(&T, m_T);

	//初始化起始预测坐标
	xx.data[0] = X; xx.data[1] = Y;
	xx.data[2] = 1; xx.data[3] = 1;
}


//通过调整测量噪声和过程噪声来改变卡尔曼的增益
void KalManm_Predict(double X, double Y, double* _X, double* _Y) {


	//测量值赋值
	yy.data[0] = X;
	yy.data[1] = Y;

	//预测状态向量
	MulMatrix(T, xx, &Pre_x);

	//预测状态协方差矩阵
	MulMatrix(T, P, &tmp1);
	TransMatrix(T, &tT);
	MulMatrix(tmp1, tT, &tmp2);
	AddMatrix(tmp2 ,Q, &Pre_p);
	//

	
	//测量值和预测试的差值
	MulMatrix(H, Pre_x, &tmp1);  //计算预测值
	SubMatrix(yy, tmp1, &Pre_y);

	//卡尔曼增益计算
	MulMatrix(H, Pre_p, &tmp1);
	TransMatrix(H, &t_H);
	MulMatrix(tmp1, t_H, &tmp2);
	AddMatrix(tmp2, R, &S);
	MulMatrix(Pre_p, t_H, &tmp1);
	InvMatrix(S, &invS);
	MulMatrix(tmp1, invS, &K);

	//更新当前状态，形成闭环
	MulMatrix(K, Pre_y, &tmp1);
	AddMatrix(Pre_x, tmp1, &x_final);
	//更新系统的协方差，形成闭环
	MulMatrix(K, H, &tmp1);
	SubMatrix(I, tmp1, &tmp2);
	MulMatrix(tmp2, Pre_p, &P);

	//本次状态的最终预测值，下次状态的输入
	xx.data[0] = x_final.data[0];
	xx.data[1] = x_final.data[1];
	
	//当前状态预测值输出
	*_X = x_final.data[0];
	*_Y = x_final.data[1];
}

3.KalManFilter函数测试

在这里，就用大家熟悉的鼠标坐标跟踪测试

#include<iostream>
#include <stdio.h>
#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"
#include"Kalman\MatFun.h"
#include"Kalman\kalman.h"
using namespace cv;
using namespace std;

const int winHeight = 800;
const int winWidth = 1500;
//二维卡尔曼滤波
//#define SRCCODE1   //OPENCV版本(调用opencv函数)
//#define SRCCODE2   //伪源码(调用Mat)
#define SRCCODE3     //源码

Point mousePosition = Point(winWidth , winHeight);
//mouse event callback
void mouseEvent(int event, int x, int y, int flags, void *param)
{
	if (event == CV_EVENT_MOUSEMOVE) {
		mousePosition = Point(x, y);
	}
}
double x, y;
//
int main(void)
{
	namedWindow("kalman2D");
	setMouseCallback("kalman2D", mouseEvent);
	Mat image2(winHeight, winWidth, CV_8UC3, Scalar(0));
	KalManInit(mousePosition.x, mousePosition.y);//初始化
	while (1)
	{
		KalManm_Predict(mousePosition.x, mousePosition.y, &x, &y);//预测
		image2.setTo(Scalar(255, 255, 255, 0));
		circle(image2, Point(x, y), 5, Scalar(0, 255, 0), 3);    //predicted point with green
		circle(image2, mousePosition, 5, Scalar(255, 0, 0), 3);    //predicted point with green

		char buf[500];
		sprintf_s(buf, 256, "predicted position:(%3d,%3d)", (int)x, (int)y);
		putText(image2, buf, Point(10, 30), CV_FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 0, 0), 1, 8);
		sprintf_s(buf, 256, "current position :(%3d,%3d)", mousePosition.x, mousePosition.y);
		putText(image2, buf, cvPoint(10, 60), CV_FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 0, 0), 1, 8);

		imshow("kalman2D", image2);
		int key = waitKey(3);
		if (key == 27) {//esc   
			break;

		}
	}
}

3.KalManFilter测试效果展示

当测量测量噪声设置为：
m_R[4] = {100000, 0,0, 100000 };
过程噪声设置为：
m_Q[16] = {
1, 0, 1, 0,
0, 1, 0, 1,
0, 0, 1, 0,
0, 0, 0, 1 };
效果如下：可以看出预测状态不是很准，因为设置测量噪声方差很大（鼠标的的准确性是很高，我们假设很大，一般根据实际情况设置），结果更偏向系统预测值。

当测量测量噪声设置为：
m_R[4] = {1000, 0,0, 1000 };
过程噪声设置为： m_Q[16] = {
1, 0, 1, 0,
0, 1, 0, 1,
0, 0, 1, 0,
0, 0, 0, 1 };
效果如下:可以明显看到准确度有所提升，是因为测量噪声减小，预测的结果更接近测量值。

总结

以上就是今天要讲的内容，本文仅仅简单介绍了Kalman的实现，其中的数学论证没有详细探讨，如果对大家有帮助，可以点赞加关注，谢谢。