C++——卡尔曼滤波理论方法及opencv中kalman应用

最新推荐文章于 2024-07-31 16:49:23 发布

注释远方

最新推荐文章于 2024-07-31 16:49:23 发布

阅读量2.5k

点赞数 3

分类专栏：算法

本文链接：https://blog.csdn.net/qinze5857/article/details/81210799

版权

算法专栏收录该内容

11 篇文章 2 订阅

订阅专栏

一、理论方法

总结起来：预测下一时刻状态，根据当前协方差求下一时刻KG，根据下一时刻测量值求最优解，更新下一时刻协方差

二、代码实现

2.1头文件：创建类KalmHeading

#include <vector>
#include <opencv2/opencv.hpp>
#include<fstream>
using namespace std;


class KalmHeading
{
	public:
		KalmHeading();
		~KalmHeading();
		void KalmInit(float& nowHeading);        //完成对x(0)的初始化
		float kalmPredict(float& nowHeading);	
		int getFileHeading();
		vector<string> split(const string &str, const string &pattern);
		void run();

		//const
		string inFileName;
		string outFileName;
		//global v
		vector<float> offHeading;
		cv::KalmanFilter * kf;      //KalmanFilter(DP,MP,CP);    
		cv::Mat measurement;   //接收测量值
		int stateNum;         //状态值1×1向量(heading)
		int measureNum ;     //测量值1×1向量heading
};

2.2实现部分

KalmHeading::KalmHeading()
{
	inFileName = "in.txt";
	outFileName = "out.txt";
	if (getFileHeading())
		cout << "成功读入数据！" << endl;
}

KalmHeading::~KalmHeading()
{
	delete kf;
}
void KalmHeading::KalmInit(float& nowHeading)
{
	stateNum = 1;
	measureNum = 1;
	kf = new cv::KalmanFilter(stateNum, measureNum, 0);			//KalmanFilter(DP,MP,CP);   
	kf->transitionMatrix = *(cv::Mat_<float>(1, 1) << 1);		//转移矩阵A
	setIdentity(kf->measurementMatrix);                         //测量矩阵H
	setIdentity(kf->processNoiseCov, cv::Scalar::all(1e-3));    //系统噪声方差矩阵Q——参数调整
	setIdentity(kf->measurementNoiseCov, cv::Scalar::all(1e-2));//测量噪声方差矩阵R——参数调整
	setIdentity(kf->errorCovPost, cv::Scalar::all(1));          //后验错误估计协方差矩阵P   
	measurement = cv::Mat::zeros(measureNum, 1, CV_32F);        //初始测量值x'(0)
	kf->statePost = *(cv::Mat_<float>(1, 1) << 1)*nowHeading;   //x(0)初始化 接口
}

float KalmHeading::kalmPredict(float& nowHeading)
{
	cv::Mat prediction = kf->predict();								//计算预测值
	cout << "predict =" << "\t" << prediction.at<float>(0) << endl; //3.0测量值
	measurement.at<float>(0) = nowHeading;							//4.0校正值
	kf->correct(measurement);
	cout << "correct =" << "\t" << kf->statePost.at<float>(0) << endl;
	return kf->statePost.at<float>(0);
}

void KalmHeading::run(){
	ofstream oFile;
	float fdata;
	oFile.open(outFileName, ios::app | ios::binary | ios::in | ios::out);
	KalmInit(offHeading[0]);
	for (int i = 1; i < offHeading.size(); i++){
		if (abs(offHeading[i]-offHeading[i-1])>300)
			KalmInit(offHeading[i]);
		fdata = kalmPredict(offHeading[i]);
		oFile << fdata << "\r\n";
		cout << "ori: " << offHeading[i] << "filter:" << fdata << endl;
	}
	oFile.close();

}

int KalmHeading::getFileHeading()
{
	ifstream iFile;
	string line;
	vector<string> tmstr;
	//1.0
	iFile.open(inFileName,ios::_Nocreate);
	if (iFile.is_open()){
		while (getline(iFile, line)){
			tmstr = split(line, ",");
			offHeading.push_back(atof(tmstr[3].c_str()));
			//cout << "line: " << line << endl;
			//cout << "tmstr: " << tmstr[3] << endl;
		}
	}
	else{
		cout << "打不开航向角文件！" << endl;
		return -1;
	}
	return 1;
}

vector<string> KalmHeading::split(const string &str, const string &pattern)
{
	//const char* convert to char*
	char * strc = new char[strlen(str.c_str()) + 1];
	strcpy(strc, str.c_str());
	vector<string> resultVec;
	char* tmpStr = strtok(strc, pattern.c_str());
	while (tmpStr != NULL)
	{
		resultVec.push_back(string(tmpStr));
		tmpStr = strtok(NULL, pattern.c_str());
	}
	delete[] strc;

	return resultVec;
}