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