二维KalManFilter滤波原理及C/C++源码
二维KalManFilter滤波原理及C/C++源码
文章目录
- 二维KalManFilter滤波原理及C/C++源码
- 前言
- 一、KalManFillter原理简介
- 二、代码实现
-
- 1.矩阵操作函数
- 2.KalManFilter实现函数
- 3.KalManFilter函数测试
- 3.KalManFilter测试效果展示
- 总结
原理介绍
前言
在工作过程中，遇到关于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
#include
#include
#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; idata[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; idata[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; idata[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; idata[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; idata[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函数测试
在这里，就用大家熟悉的鼠标坐标跟踪测试
#include3.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的实现，其中的数学论证没有详细探讨，如果对大家有帮助，可以点赞加关注，谢谢。