C++实现卡尔曼滤波

参数初始化

	A << 1.0000, 1.0000, 0.0000, 0.0000,
		0.0000, 1.0000, 0.0000, 0.0000,
		0.0000, 0.0000, 1.0000, 1.0000,
		0.0000, 0.0000, 0.0000, 1.0000;
	P << 50000.0000, 0.0000, 0.0000, 0.0000,
		0.0000, 30000.0000, 0.0000, 0.0000,
		0.0000, 0.0000, 50000.0000, 0.0000,
		0.0000, 0.0000, 0.0000, 30000.0000;
	Q << 1.0000, 0.0000, 0.0000, 0.0000,
		0.0000, 1.0000, 0.0000, 0.0000,
		0.0000, 0.0000, 1.0000, 0.0000,
		0.0000, 0.0000, 0.0000, 1.0000;
	R << 150.0000, 0.0000 
		0.0000, 150.0000;
	H << 1, 0, 0, 0,
		0, 0, 1, 0;

三维动态曲线

QVector<QVector<double>> dataVec;
	static int count = 0;
	readTxtData("data/res_3d.txt", 3, dataVec);
	static QScatterDataArray * data_3d = new QScatterDataArray();
	if(count==0)
		ui.m_3dSeries->dataProxy()->addItems(*data_3d);
	if (count < dataVec.size())
	{
		data_3d->append(QVector3D(dataVec[count][0], dataVec[count][2], dataVec[count][1]));
	}
	count++;
	ui.m_3dSeries->dataProxy()->resetArray(data_3d);