卡尔曼滤波预测三维坐标点

在网上找了很多卡尔曼滤波的代码，后来觉得文心一言生成的代码更简单容易理解。

修改了很多遍，勉强能运行，放在这里算是记录。

objectPoints是一个存储三维坐标点的矩阵，遍历矩阵预测更新坐标点。

卡尔曼滤波适用于线性系统，我试了几组数据发现偏差很大，后面溉用扩展卡尔曼滤波了。

// 添加第一个点 (X, Y, Z)  
objectPoints.push_back(cv::Point3f(0.378343f, 0.3473857f, 0.786856f)); // 原点  

// 添加第二个点  
objectPoints.push_back(cv::Point3f(-0.82075f, 0.9275763f, 0.748373f)); // X轴正方向上的一个单位点  

// 添加第三个点  
objectPoints.push_back(cv::Point3f(0.3775489f, -0.24635f, 0.8435f)); // Y轴正方向上的一个单位点  

// 添加第四个点  
objectPoints.push_back(cv::Point3f(-0.47589357f, -0.777578f, 0.3874f)); // Z轴正方向上的一个单位点（尽管在图像平面上Z通常为0）  

cout << "测试数据:" << objectPoints << endl;
int stateSize = 6; // 状态大小：位置(x, y) + 速度(vx, vy)  
int measureSize = 3; // 测量大小：位置(x, y)  
int controlSize = 0; // 没有控制输入 
cv::KalmanFilter kalmanFilter(stateSize, measureSize, controlSize, CV_32F);

// 设置初始状态  
cv::Mat initialState = Mat::zeros(stateSize, 1, CV_32F);
//initialState.setTo(cv::Scalar(0)); // 所有状态设置为0（位置在原点，速度为0）  
kalmanFilter.statePost = initialState;
// 转移矩阵 F  
// 假设dt是时间步长（例如，0.1秒）  
double dt = 0.1;
cv::Mat F = (cv::Mat_<float>(stateSize, stateSize) <<
				1, 0, dt, 0, 0, 0.5 * dt * dt,
				0, 1, 0, dt, 0, 0.5 * dt * dt,
				0, 0, 1, 0, dt, dt * dt,
				0, 0, 0, 1, 0, dt,
				0, 0, 0, 0, 1, 1,
				0, 0, 0, 0, 0, 1);
kalmanFilter.transitionMatrix = F;

// 测量矩阵 H  
cv::Mat H = (cv::Mat_<float>(measureSize, stateSize) <<
				1, 0, 0, 0, 0, 0,
				0, 1, 0, 0, 0, 0);
kalmanFilter.measurementMatrix = H;

// 过程噪声协方差矩阵 Q  
// 根据你的应用调整这些值  
cv::Mat Q = (cv::Mat_<float>(stateSize, stateSize) <<
				0.01, 0, 0, 0, 0, 0,
				0, 0.01, 0, 0, 0, 0,
				0, 0, 0.01, 0, 0, 0,
				0, 0, 0, 0.01, 0, 0,
				0, 0, 0, 0, 0.01, 0,
				0, 0, 0, 0, 0, 0.01);
kalmanFilter.processNoiseCov = Q;

// 测量噪声协方差矩阵 R  
// 根据你的测量设备的精度调整这些值  
cv::Mat R = (cv::Mat_<float>(measureSize, measureSize) <<
				0.01, 0,
				0, 0.01
				);
kalmanFilter.measurementNoiseCov = R;

 初始误差协方差矩阵 P  
 初始时可能不确定，所以设置为较大的值  
cv::Mat P = (cv::Mat_<float>(stateSize, stateSize) <<
				1, 0, 0, 0,
				0, 1, 0, 0,
				0, 0, 1, 0,
				0, 0, 0, 1
				) * 1e3;
kalmanFilter.errorCovPost = P;

// 用于存储预测的坐标点  
std::vector<cv::Point2f> predictedPoints;
// 准备测量值（仅位置，速度为0，因为卡尔曼滤波器将基于位置进行预测） 
for (size_t i = 0; i < objectPoints.size(); ++i)
{
				cv::Mat measurement = (cv::Mat_<float>(measureSize, 1) << objectPoints[i].x, objectPoints[i].y,0);
				
				// 预测  
				cv::Mat prediction = kalmanFilter.predict();
				cout <<"prediction" << prediction << endl;
				//计算测量值
				measurement.at<float>(0) = (float)objectPoints[i].x;
				measurement.at<float>(1) = (float)objectPoints[i].y;
				cout << "measurement" <<measurement.at<float>(0) << "\t" << measurement.at<float>(1)<< endl;
				// 校正（使用当前测量值）  
				kalmanFilter.correct(measurement);
				
				// 现在kalmanFilter.statePost包含校正后的状态估计（位置和速度）  
				// 提取位置信息  
				float predictedX = kalmanFilter.statePost.at<float>(0);
				float predictedY = kalmanFilter.statePost.at<float>(1);
				cout << "xxxx" << predictedX << "yyyy" << predictedY << endl;
				predictedPoints.push_back(cv::Point2f(predictedX, predictedY));
				// 确保预测的位置在图像范围内  
				predictedX = std::max(0.0f, std::min(predictedX, copy.cols - 1.0f));
				predictedY = std::max(0.0f, std::min(predictedY, copy.rows - 1.0f));
				// 在图像上绘制点  
				// 存储预测的坐标点以供后续绘制  
}
// 在图像上绘制一系列的点以显示轨迹  
for (const auto& point : predictedPoints)
{
				cv::circle(copy, point, 5, cv::Scalar(0, 255, 255), -1); // 黄色圆点，-1表示填充  
				std::cout << "Predicted point: (" << point.x << ", " << point.y << ")" << std::endl;
}

objectPoints.clear();