卡尔曼滤波并可视化（C++实现）

前言

之前写过卡尔曼滤波器MATLAB实现（从一维到三维），介绍了卡尔曼滤波原理，并使用MATLAB实现。这次使用C++实现，当然，可视化效果没那么好，只做了一维的。

使用 VS code 2019 ，还要自己额外安装 EasyX 用于绘图。EasyX的安装和使用很简单的，可以参考这篇文章。

首先看效果，白色为滤波前数据，绿色为滤波后数据。

代码

头文件包含数据及数据大小定义，源文件主要实现滤波与可视化。

// mydata.h
#ifndef _MY_DATA_H_
#define _MY_DATA_H_

#define DATA_LEN 300

float kf_data[DATA_LEN] = { 0.1174, -0.0019, 0.0532, -0.0052, 0.0792, 0.1363, 0.1089, 0.0961, 0.1581, 0.4877, 0.3557, 0.3857, 0.4820, 0.5296, 0.4860, 0.2959, 0.3550, 0.3749, 0.3898, 0.2488, 0.1347, -0.0261, -0.0987, 0.2222, 0.4409, 0.4615, 0.3932, 0.3323, 0.5386, 0.5369, 0.4115, 0.5649, 0.7561, 0.5093, 0.4638, 0.3814, 0.4784, 0.3202, 0.2773, 0.1743, 0.2198, 0.0742, 0.1152, -0.0237, -0.0649, 0.0625, 0.1227, 0.1898, -0.1294, -0.2487, -0.3163, -0.2675, 0.4983, 0.6819, 0.5525, 0.2904, -0.2463, 0.0794, 0.0762, 0.0704, 0.1859, 0.0434, -0.3273, -0.3022, -0.5392, -0.4765, -0.5303, -0.5401, -0.5699, -0.5580, -0.7264, -0.7046, -0.4456, -0.4940, 0.1469, 0.5419, 0.4869, 0.5408, 0.4614, 0.3825, -0.3692, -0.6335, -0.6845, -0.6573, -0.7161, -0.7995, -0.6044, -0.6532, -0.8016, -0.6881, -0.6734, -0.6731, -0.4569, -0.6865, -0.7035, -0.4034, -0.2458, -0.1256, 0.1640, -0.1251, -0.5283, -0.4266, -0.5526, -0.5275, -0.4951, -0.4302, -0.4498, -0.0855, -0.1180, -0.2548, 0.1581, 0.1166, -0.0614, 0.0456, 0.3795, 0.1851, 0.3925, 0.2438, 0.4793, 0.3003, 0.3054, 0.4763, 0.7500, 0.6591, 0.4540, 0.3264, 0.5614, 0.3863, 0.3308, 0.8122, 0.7264, 0.6771, 0.2349, 0.2102, 0.2385, -0.0868, -0.1262, -0.2555, -0.4423, -0.3316, -0.3533, -0.3039, -0.4744, -0.7096, -0.6131, -0.3837, -0.3448, 0.1938, 0.0726, 0.0885, 0.0583, 0.0148, 0.1028, 0.0416, 0.2977, 0.5544, 0.5124, 0.5504, 0.6547, 0.5631, 0.5160, 0.7760, 0.6639, 0.7048, 0.5819, 0.8298, 0.8220, 0.8534, 0.6238, 0.7626, 0.4513, -0.0842, -0.0407, -0.1669, 0.0581, -0.1169, 0.0348, 0.2258, 0.3072, 0.7321, 0.6453, 0.4947, 0.2677, -0.0146, 0.0265, 0.2798, 0.1140, -0.1063, -0.0199, 0.1113, -0.0348, 0.3090, 0.3922, 0.4630, 0.2592, -0.1089, -0.2212, -0.2454, -0.2820, 0.0559, 0.0431, 0.0005, 0.0940, 0.1331, 0.0790, 0.0740, -0.0477, -0.1016, -0.0503, -0.3198, -0.2967, -0.3613, -0.7015, -0.7248, -0.4111, -0.3013, -0.3284, -0.4848, -0.2817, 0.0093, 0.0769, -0.6473, -0.9491, -0.9044, -1.0139, -0.8124, -0.7626, -0.8464, -0.6711, -0.8015, -0.7364, -1.1220, -0.9984, -1.0211, -0.7865, -0.6402, -0.3361, -0.1779, 0.1358, 0.1273, 0.1302, 0.0690, 0.0385, 0.0244, 0.0141, 0.0120, -0.0086, -0.0176, 0.0360, 0.1438, 0.2318, 0.2841, 0.2495, 0.2361, 0.2206, 0.1711, 0.0752, 0.0485, 0.0673, 0.0485, 0.0690, 0.0103, 0.0277, 0.0085, 0.0105, 0.0291, 0.0149, 0.0177, 0.0308, 0.0462, 0.0493, 0.0618, 0.0708, 0.0827, 0.0792, 0.0543, 0.0198, 0.0097, 0.0110, 0.0196, 0.0306, 0.0137, 0.0000, 0.0035, 0.0016, 0.0085, 0.0122, 0.0129, -0.0034, -0.0011, 0.0290, 0.0326, 0.0307, 0.0108, 0.0000, 0.0171, 0.0114, -0.0001, -0.0111, -0.0086 };

#endif

// mykarman.cpp

#include <iostream>
#include <graphics.h>
#include <time.h>
#include <conio.h>
#include "mydata.h"									// 包含原始数据

using namespace std;

#define WIN_W		640								// 窗口宽度						
#define WIN_H		480								// 窗口高度
#define GRA_W		600								// 图像最大宽度	
#define GRA_H		300								// 图像最大高度
#define GRA_TOP		WIN_H / 2 + GRA_H / 2			// 图像顶部坐标
#define GRA_BOT		WIN_H / 2 - GRA_H / 2			// 图像底部坐标
#define GRA_LEF		WIN_W / 2 - GRA_W / 2			// 图像最左坐标
#define GRA_RIG		WIN_W / 2 + GRA_W / 2			// 图像最右坐标

void karman_filter(const float* din, float* dout);  // 卡尔曼滤波函数
void mapping(const float* din, float* dout, const float* din2, float* dout2, int BOT_LEF, int TOP_RIG); // 两组数据等比例线性映射函数
void draw_2curves(const float* x1, const float* y1, const float* x2, const float* y2); // 绘制两条曲线

int main()
{
	initgraph(WIN_W, WIN_H);				// 初始化绘图窗口大小

	float x_axis[DATA_LEN];					// 横坐标
	float dat_unf[DATA_LEN];				// 滤波前数据横坐标
	float dat_fil[DATA_LEN];				// 滤波后数据纵坐标

	for (int i = 0; i < DATA_LEN; i++)
	{
		x_axis[i] = i;						// 横坐标数据 1~300
		dat_unf[i] = kf_data[i];			// 未滤波数据
	}

	karman_filter(dat_unf, dat_fil);		// 卡尔曼滤波

	draw_2curves(x_axis, dat_unf, x_axis, dat_fil); // 结果可视化

	_getch();
	closegraph();

	return 0;
}

/* 卡尔曼滤波函数							*/
/* 输入待滤波数据 din, 输出滤波后数据 dout	*/
void karman_filter(const float* din, float* dout)
{
	float dt = 1;					// 时间步长
	float A = 1;					// 状态转移矩阵
	float P = 1;					// 协方差初始值
	float Q = 0.001;				// 系统噪声
	float R = 0.01;					// 测量噪声
	float H = 1;					
	float K;

	float x[300] = { 0 };			// 首项赋值为0
	float z[300];

	for (int i = 0; i < 300; i++)
		z[i] = din[i];

	for (int i = 1; i < DATA_LEN; i++)
	{
		x[i] = A * x[i - 1];					// 卡尔曼滤波公式 1
		P = A * P * A + Q;						// 卡尔曼滤波公式 2
		K = P * H / (H * P * H + R);				// 卡尔曼滤波公式 3
		x[i] = x[i] + K * (z[i] - H * x[i]);	// 卡尔曼滤波公式 4
		P = (1 - K * H) * P;					// 卡尔曼滤波公式 5
	}

	for (int i = 0; i < DATA_LEN; i++)			// 赋值给输出
		dout[i] = x[i];
}

/* 数组线性映射到 [GRA_BOT, GRA_TOP] 区间							*/
/* 输入数组 din 和 din2												*/
/* 映射后的数据分别为 dout, dout2									*/
/* 实际上取的是 din 的范围映射得到比例系数 k,b ，再应用于 din2		*/
void mapping(const float* din, float* dout, const float* din2, float* dout2, int BOT_LEF, int TOP_RIG)
{
	float ma = din[0];
	float mi = din[0];

	// 获取最大最小值
	for (int i = 0; i < DATA_LEN; i++)
	{
		if (din[i] > ma)
			ma = din[i];
		else if (din[i] < mi)
			mi = din[i];
		else
			continue;
	}

	// 计算系数
	float k = (TOP_RIG - BOT_LEF) / (ma - mi);
	float b = TOP_RIG - k * ma;

	// 输出结果
	for (int i = 0; i < DATA_LEN; i++)
		dout[i] = k * din[i] + b;

	for (int i = 0; i < DATA_LEN; i++)
		dout2[i] = k * din2[i] + b;
}

/* 绘制两条连续曲线												*/
/* 两条线横纵坐标分别为 数组(x1, y1), 数组(x2, y2)				*/
void draw_2curves(const float* x1, const float* y1, const float* x2, const float* y2)
{
	int i;

	float x1out[DATA_LEN];
	float x2out[DATA_LEN];
	float y1out[DATA_LEN];
	float y2out[DATA_LEN];

	//cout << GRA_TOP << "," << GRA_BOT << "," << GRA_LEF << "," << GRA_RIG << endl;
	mapping(x1, x1out, x2, x2out, GRA_LEF, GRA_RIG);					// 映射横坐标到 [GRA_BOT, GRA_TOP] 区间
	mapping(y1, y1out, y2, y2out, GRA_BOT, GRA_TOP);					// 映射纵坐标到 [GRA_LEF, GRA_RIG] 区间

	setcolor(WHITE);																// 白色
	for (i = 0; i < DATA_LEN - 1; i++)
		line((int)x1out[i], (int)y1out[i], (int)x1out[i + 1], (int)y1out[i + 1]);	// 绘制第一条曲线

	setcolor(GREEN);																// 绿色
	for (i = 0; i < DATA_LEN - 1; i++)
		line((int)x2out[i], (int)y2out[i], (int)x2out[i + 1], (int)y2out[i + 1]);	// 绘制第二条曲线
}