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

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



原理介绍


前言

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


一、KalManFillter原理简介

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

二、代码实现

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的实现,其中的数学论证没有详细探讨,如果对大家有帮助,可以点赞加关注,谢谢。

  • 30
    点赞
  • 36
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 1
    评论
二维Kalman滤波器是一种用于状态估计和信号滤波的理论方法。它结合了系统的动态模型和测量的观测数据,可以在存在测量噪声和系统不确定性的情况下,通过递归计算得到状态的最优估计。 该滤波器的原理可以简述为以下几个步骤: 1. 初始化: - 初始化状态向量(x)和协方差矩阵(P)的估计值。 - 初始化测量噪声协方差矩阵(R)和系统噪声协方差矩阵(Q)的估计值。 2. 预测阶段: - 根据系统的动态模型,使用状态转移矩阵(A)和系统噪声协方差矩阵(Q)来预测当前时刻的状态向量和协方差矩阵。 3. 更新阶段: - 根据当前的测量值和测量噪声协方差矩阵(R),计算观测模型矩阵(C)和卡尔曼增益(K)。 - 使用卡尔曼增益、观测模型矩阵和测量值,更新状态向量和协方差矩阵的估计值。 4. 重复预测和更新阶段,直到完成所有的测量数据处理。 以下是一个使用C/C++编写的二维Kalman滤波器的简单源代码示例: ```c #include <iostream> #include <opencv2/opencv.hpp> using namespace std; using namespace cv; int main() { // 定义状态向量、协方差矩阵等变量 Mat x(2, 1, CV_32FC1); Mat P(2, 2, CV_32FC1); Mat A(2, 2, CV_32FC1); Mat Q(2, 2, CV_32FC1); Mat H(1, 2, CV_32FC1); Mat R(1, 1, CV_32FC1); Mat I(2, 2, CV_32FC1); Mat z(1, 1, CV_32FC1); Mat K(2, 1, CV_32FC1); // 初始化估计值 x.at<float>(0, 0) = 0; x.at<float>(1, 0) = 0; P.at<float>(0, 0) = 1; P.at<float>(0, 1) = 0; P.at<float>(1, 0) = 0; P.at<float>(1, 1) = 1; // 初始化模型参数和噪声协方差矩阵 A.at<float>(0, 0) = 1; A.at<float>(0, 1) = 1; A.at<float>(1, 0) = 0; A.at<float>(1, 1) = 1; Q.at<float>(0, 0) = 0.1; Q.at<float>(0, 1) = 0; Q.at<float>(1, 0) = 0; Q.at<float>(1, 1) = 0.1; H.at<float>(0, 0) = 0.5; H.at<float>(0, 1) = 0.5; R.at<float>(0, 0) = 1; I.at<float>(0, 0) = 1; I.at<float>(0, 1) = 0; I.at<float>(1, 0) = 0; I.at<float>(1, 1) = 1; // 循环处理数据 for (int i = 0; i < 100; i++) { // 预测阶段 x = A * x; P = A * P * A.t() + Q; // 更新阶段 z.at<float>(0, 0) = i + rand() % 10; // 生成随机测量值 K = P * H.t() * (H * P * H.t() + R).inv(); x = x + K * (z - H * x); P = (I - K * H) * P; // 输出估计值 cout << "Estimated state at time " << i << ":" << endl; cout << "x = " << x << endl; cout << "P = " << P << endl; } return 0; } ``` 这是一个简单的二维Kalman滤波器示例,其中包括状态估计的初始化和更新阶段的实现。通过不断的预测和更新,可以得到最优的状态估计值。请注意,上述代码仅供参考,实际应用中可能需要根据具体的需求进行调整和完善。
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

_SimpleLife

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值