kfPredict.h
#include <opencv2/opencv.hpp>
#include <iostream>
using namespace cv;
using namespace std;
#define MAXROUNDCOUNT 1024
class EKFPredict
{
public:
EKFPredict();
~EKFPredict();
public:
/**
* @brief 拓展卡尔曼预测主流程
* 【1】累计7个点与他们对应的时间戳 条件:[1]type一致 [2]framecount数连续 [3]上一个点和此点像素距离小于80
* 【2】进卡尔曼滤波预测下一个点
* @param _timestamp 时间戳
* @param _framecount 帧计数
* @return _thisPoint 此帧检测的能量机关激活点
*
* @author hmy
* @date 2023.9.18
*/
Point2f EKFPredict_Main(double& _timestamp ,int &_framecount, Point2f &_thisPoint, int &_thisType);
/**
* @brief 累计观测
* 【1】framecount要连续 否则清空
* 【2】上一个点和此点距离不能大于12 否则清空
* 【3】符合条件且已有Vector满4后 后三点前挪一位放入第四个点
* @param _timestamp 时间戳
* @param _framecount 帧计数
* @return _thisPoint 此帧检测的能量机关激活点
*
* @author hmy
* @date 2023.9.19
*/
int EKFPredict_Accumulate(int& _framecount, Point2f& _thisPoint,int& _thisType, double& _timestamp);
/**
* @brief 设置EKF滤波器参数
* @param
*
* @author hmy
* @date 2023.9.19
*/
void EKFPredict_EkfSetting();
/**
* @brief 设置第一个观测值
*
* @param
*
* @authorhmy
* @date 2023.9.19
*/
void EKFPredict_EkfSetFirstState(Point2f& _firstPoint, float & _vx,float& _vy);
Point2f EKFPredict_Predict(vector<double> &_timestampofpts , vector<Point2f> &_ptsofobervation);
public:
int m_lastCount;
int m_currentCount;
int m_lastType;
int m_currentType;
Point2f m_lastPoint;
Point2f m_currentPoint;
vector<Point2f> m_ObservationPoints;
vector<double> m_TimestampofPoints;
// 创建卡尔曼滤波器
KalmanFilter m_ekf;
Mat m_F;
Mat m_H;
// m_AccmulateNum 是累计数,累计符合条件的Point
int m_AccmulateNum;
// m_RenewJudgement只会等于0/4,开启下一次预测的count 每次切换目标则m_RenewJudgement = 0 ,用于判断如何更新卡尔曼滤波器的初始state ;
int m_RenewJudgement;
// 下一个点若是连续时,则第一个statepose用列表[0] [1]之间的vx vy再进行更替
float m_vx01, m_vy01;
int m_fps;
};
static float distanceOfPoints(const Point2f& p1, const Point2f& p2) {
float dx = p2.x - p1.x;
float dy = p2.y - p1.y;
return sqrt(dx * dx + dy * dy);
}
kfPredict.cpp
#include "kfPredict.h"
EKFPredict::EKFPredict()
{
}
EKFPredict::~EKFPredict()
{
}
Point2f EKFPredict::EKFPredict_Main(double& _timestamp, int& _framecount, Point2f& _thisPoint, int&_thisType)
{
/* 如果累计了7个连续的点,则将点集和时间戳都放进卡尔曼滤波器 */
int AccmulateNum = EKFPredict_Accumulate(_framecount, _thisPoint,_thisType,_timestamp);
Point2f PreDictPoint;
if (AccmulateNum == 7)
{
float vx = 0;
float vy = 0;
vector < Point2f > PredicePoints = m_ObservationPoints;
vector <double > PredictTimes = m_TimestampofPoints;
EKFPredict_EkfSetting(); ///< 首次满7,重新设置滤波器
EKFPredict_EkfSetFirstState(m_ObservationPoints[0], vx, vy);
PreDictPoint = EKFPredict_Predict(PredictTimes,PredicePoints);
return PreDictPoint;
}
else if (AccmulateNum > 7) {
vector < Point2f > PredicePoints = m_ObservationPoints;
vector <double > PredictTimes = m_TimestampofPoints;
//EKFPredict_EkfSetting();
EKFPredict_EkfSetFirstState(m_ObservationPoints[0], m_vx01, m_vy01);
PreDictPoint = EKFPredict_Predict(PredictTimes, PredicePoints);
return PreDictPoint;
}
//不符合 则返回原点
return _thisPoint;
}
int EKFPredict::EKFPredict_Accumulate(int& _framecount, Point2f& _thisPoint, int& _thisType, double& _timestamp)
{
/* 第一次计数 初始化前后计数和前后点 */
if (_framecount == 0)
{
m_lastType = m_currentType = _thisType;
m_lastCount = m_currentCount = _framecount;
m_lastPoint = m_currentPoint = _thisPoint;
m_AccmulateNum = 0;
return m_AccmulateNum;
}
else /* 第二次开始 用前后点先后赋值再进行作差 */
{
m_lastCount = m_currentCount;
m_currentCount = _framecount;
m_lastPoint = m_currentPoint;
m_currentPoint = _thisPoint;
/* 这里的三种情况可以用一条或判断语句全部写完 但是为了方便看,我这里分开写*/
/* 情况一 :前后类型不一致 返回0*/
if (_thisType != m_lastType)
{
m_AccmulateNum = 0;
m_ObservationPoints.clear();
m_TimestampofPoints.clear();
return m_AccmulateNum;
}
/* 情况二:前后帧数差 > 1 返回0*/
/* 轮的次数是1024 (0~1023) 因此会出现前1023 后 0 的情况*/
if ((m_currentCount - m_lastCount > 2) && (m_currentCount - m_lastCount != -(MAXROUNDCOUNT -1)))
{
m_AccmulateNum = 0;
m_ObservationPoints.clear();
m_TimestampofPoints.clear();
return m_AccmulateNum;
}
/* 情况三:前后中心点距离 > 80 返回0*/
if (distanceOfPoints(m_currentPoint, m_lastPoint) > 80)
{
m_ObservationPoints.clear();
m_TimestampofPoints.clear();
m_AccmulateNum = 0;
return m_AccmulateNum;
}
m_AccmulateNum = m_AccmulateNum + 1;
if (m_AccmulateNum <= 7)
{
if (m_AccmulateNum == 1) {
m_ObservationPoints.clear();
m_TimestampofPoints.clear();
}
m_ObservationPoints.push_back(_thisPoint);
m_TimestampofPoints.push_back(_timestamp);
}
else {
m_ObservationPoints.erase(m_ObservationPoints.begin());
m_ObservationPoints.push_back(_thisPoint);
m_TimestampofPoints.erase(m_TimestampofPoints.begin());
m_TimestampofPoints.push_back(_timestamp);
}
return m_AccmulateNum;
}
}
void EKFPredict::EKFPredict_EkfSetting()
{
KalmanFilter InitKF(4, 2, 0);
// 定义状态转移矩阵F
m_F = (Mat_<float>(4, 4) <<
1 , 0 , 1 , 0 ,
0 , 1 , 0 , 1 ,
0 , 0 ,1.6 , 0 ,
0 , 0 , 0.5 , 1.6);
// 定义测量矩阵H
m_H = Mat::eye(2, 4, CV_32F);
// 设置过程噪声协方差矩阵和测量噪声协方差矩阵
setIdentity(InitKF.processNoiseCov, Scalar::all(1e-1));
setIdentity(InitKF.measurementNoiseCov, Scalar::all(1e-2));
setIdentity(InitKF.transitionMatrix);
setIdentity(InitKF.measurementMatrix);
m_ekf = InitKF;
}
void EKFPredict::EKFPredict_EkfSetFirstState(Point2f& _firstPoint, float& _vx, float& _vy)
{
// 定义初始状态向量
Mat state(4, 1, CV_32F); // [x, y, vx, vy]
state.at<float>(0) = _firstPoint.x;
state.at<float>(1) = _firstPoint.y;
state.at<float>(2) = _vx;
state.at<float>(3) = _vy;
m_ekf.statePost = state;
}
Point2f EKFPredict::EKFPredict_Predict(vector<double>& _timestampofpts, vector<Point2f>& _ptsofobervation)
{
Mat measurement = Mat::zeros(2, 1, CV_32F); // 设置测量值 [x, y]
//cout << " -------------error start 1 ---------------" << endl;
double dt1 = _timestampofpts[1] - _timestampofpts[0];
// 更新状态转移矩阵
//m_ekf.transitionMatrix.at<float>(0, 2) = dt1*5;
//m_ekf.transitionMatrix.at<float>(1, 3) = dt1 * (1.0 / 3.0)*5;
//cout << " -------------error start 2 ---------------" << endl;
// 使用前7帧的数据更新滤波器的状态
for (int i = 0; i < 7; i++)
{
if (i > 1)
{
double nextTimestamp = _timestampofpts[i] + (_timestampofpts[i] - _timestampofpts[i-1]);
float dt = nextTimestamp - _timestampofpts[i];
m_F.at<float>(0, 2) = dt;
m_F.at<float>(1, 3) = dt;
}
measurement.at<float>(0) = _ptsofobervation[i].x;
measurement.at<float>(1) = _ptsofobervation[i].y;
// 预测步骤
Mat prediction = m_F * m_ekf.statePost;
m_ekf.statePre = prediction;
m_ekf.errorCovPre = m_F * m_ekf.errorCovPost * m_F.t() + m_ekf.processNoiseCov;
// 更新步骤
Mat y = measurement - m_H * m_ekf.statePre;
Mat S = m_H * m_ekf.errorCovPre * m_H.t() + m_ekf.measurementNoiseCov;
Mat K = m_ekf.errorCovPre * m_H.t() * S.inv();
m_ekf.statePost = m_ekf.statePre + K *y;
m_ekf.errorCovPost = (Mat::eye(4, 4, CV_32F) - K * m_H) * m_ekf.errorCovPre;
//cout << " -------------error end ---------------" << endl;
}
Mat prediction = m_F * m_ekf.statePost;
// 提取预测结果
Point2f nextPoint(prediction.at<float>(0), prediction.at<float>(1));
//m_vx01 = (_ptsofobervation[1].x- _ptsofobervation[0].x)/ (m_fps*(_timestampofpts[1] - _timestampofpts[0]));
m_vx01 = prediction.at<float>(2);
//m_vy01 = (_ptsofobervation[1].y- _ptsofobervation[0].y)/ (m_fps*(_timestampofpts[1] - _timestampofpts[0]));
m_vy01 = prediction.at<float>(3);
//cout << "下一帧位置:" << nextPoint << endl;
return nextPoint;
}