扩展卡尔曼滤波学习（二）

果然，代码被我改乱了，想让它预测objecpoints中的x和y的值，弄来弄去也没成功，感觉还是得学习计算方法写自己的计算程序，网络上大多数都是用Eigen算的，不挣扎了，下这个库跟着做还例子多一些。

先记录一下~~~ ，说不定后面还要用到又找不到^_^

#include <stdio.h>
#include <math.h>
#include <iostream>
#include <opencv2/video/tracking.hpp>// 包含卡尔曼滤波器  
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;
// 状态向量维度
#define STATE_DIM 4
#define OBJECT_POINT_COUNT 4

// 假设每个objectPoint的状态是(x, y, vx, vy)，其中vx和vy分别是x和y方向的速度
struct ObjectState {
	double x, y, vx, vy;
};
// 状态转移函数
ObjectState stateTransition(const ObjectState& prevState, double dt) {
	ObjectState nextState;
	nextState.x = prevState.x + dt * prevState.vx;
	nextState.y = prevState.y + dt * prevState.vy;
	nextState.vx = prevState.vx; // 假设速度不变，实际情况可能需要考虑加速度
	nextState.vy = prevState.vy;
	return nextState;
}
// 观测函数，这里简化处理，直接返回当前状态作为观测
ObjectState observationFunction(const ObjectState& state) {
	return state;
}
// 状态向量
typedef struct {
	double x;
	double y;
	double vx;
	double vy;
} StateVector;     //定义了一个名为StateVector的类型，它包含两个双精度浮点数x和y
//
 观测点结构体
//typedef struct {
//	double x;
//	double y;
//} ObjectPoint;

 预测步骤中的非线性状态转移函数
//StateVector stateTransition(StateVector prevState, double dt) {
//	StateVector nextState;
//	nextState.x = prevState.x + dt * cos(prevState.y);
//	nextState.y = prevState.y + dt * sin(prevState.x);
//	return nextState;
//}//stateTransition接收当前状态prevState和时间步长dt，并返回下一个状态nextState。

 观测函数
//StateVector observationFunction(const ObjectPoint& observation) {
//	return StateVector{ observation.x, observation.y };
//}

 观测步骤中的非线性观测函数
//StateVector observationFunction(StateVector state) {
//	return state;
//}

// 2x2 矩阵求逆  
void matrixInverse(double A[STATE_DIM][STATE_DIM], double invA[STATE_DIM][STATE_DIM]) {
	double det = A[0][0] * A[1][1] - A[0][1] * A[1][0];
	if (det == 0) {
		// 矩阵不可逆  
		// 这里可以添加错误处理代码  
		return;
	}
	invA[0][0] = A[1][1] / det;
	invA[0][1] = -A[0][1] / det;
	invA[1][0] = -A[1][0] / det;
	invA[1][1] = A[0][0] / det;
}
void matrixMultiply(double A[][STATE_DIM], double B[][STATE_DIM], double C[][STATE_DIM]) {
	for (int i = 0; i < STATE_DIM; ++i) {
		for (int j = 0; j < STATE_DIM; ++j) {
			C[i][j] = 0;
			for (int k = 0; k < STATE_DIM; ++k) {
				C[i][j] += A[i][k] * B[k][j];
			}
		}
	}
}

void matrixTranspose(double A[][STATE_DIM], double B[][STATE_DIM]) {
	for (int i = 0; i < STATE_DIM; ++i) {
		for (int j = 0; j < STATE_DIM; ++j) {
			B[i][j] = A[j][i];
		}
	}
}



// 扩展卡尔曼滤波主函数
void extendedKalmanFilter(StateVector* state, double dt, double processNoise, double measurementNoise, const ObjectState& observation) {
	
	// 状态方程的雅可比矩阵
	double F[STATE_DIM][STATE_DIM] = { {1, -dt * sin(state->y)}, {dt * cos(state->x), 1} };

	// 观测方程的雅可比矩阵
	double H[STATE_DIM][STATE_DIM] = { {1, 0}, {0, 1} };

	// 过程噪声协方差矩阵
	double Q[STATE_DIM][STATE_DIM] = { {processNoise, 0}, {0, processNoise} };

	// 测量噪声协方差矩阵
	double R[STATE_DIM][STATE_DIM] = { {measurementNoise, 0}, {0, measurementNoise} };

	// 初始化预测状态和更新状态  
	StateVector xUpdated = { 0.0, 0.0 };   // 假设初始更新状态也为 (0, 0)
	double P[STATE_DIM][STATE_DIM] = { {1, 0}, {0, 1} };
	
	
	
	// 预测步骤
	double F_transposed[STATE_DIM][STATE_DIM];
	matrixTranspose(F, F_transposed);

	// 预测协方差矩阵  
	StateVector xPredicted = stateTransition(*state, dt);
	double P_predicted[STATE_DIM][STATE_DIM]; // 初始化或从上一步更新得到  
	// 计算FP  
	double FP[STATE_DIM][STATE_DIM];
	matrixMultiply(F, P, FP);
	// 计算P' = FPF^T + Q  
	for (int i = 0; i < STATE_DIM; ++i) {
		for (int j = 0; j < STATE_DIM; ++j) {
			P_predicted[i][j] = 0;
			for (int k = 0; k < STATE_DIM; ++k) {
				P_predicted[i][j] += FP[i][k] * F_transposed[k][j]; // 使用F_transposed来计算F^T  
			}
			// 加上过程噪声协方差Q（Q应该是一个已定义的矩阵）  
			P_predicted[i][j] += Q[i][j];
		}
	}

	// 更新步骤
	
	double H_transposed[STATE_DIM][STATE_DIM];
	matrixTranspose(H, H_transposed);
	// 计算卡尔曼增益K  
	double S[STATE_DIM][STATE_DIM];
	matrixMultiply(H, P_predicted, S);// S = H * P_predicted  
	for (int i = 0; i < STATE_DIM; ++i) {
		for (int j = 0; j < STATE_DIM; ++j) {
			for (int k = 0; k < STATE_DIM; ++k) {
				S[i][j] += R[i][k] * H[k][j]; // S = H * P_predicted * H^T + R (但这里我们先做乘法，稍后加上R)  
			}
			S[i][j] += R[i][j];
		}
	}
	
	double invS[STATE_DIM][STATE_DIM];
	matrixInverse(S, invS);
	double K[STATE_DIM][STATE_DIM];
	matrixMultiply(P_predicted, H_transposed, K); // K = P_predicted * H^T  
	matrixMultiply(K, invS, K); // K = K * inv(S)  
	
	// 计算残差  
	StateVector residual;
	residual.x = observation.x - state->x;
	residual.y = observation.y - state->y;

	// 更新状态估计  
	state->x += K[0][0] * residual.x + K[1][0] * residual.y;
	state->y += K[0][1] * residual.x + K[1][1] * residual.y;
	// 更新协方差矩阵P  
    // 先计算 I - KH  
	double IKH[STATE_DIM][STATE_DIM];
	for (int i = 0; i < STATE_DIM; ++i) {
		for (int j = 0; j < STATE_DIM; ++j) {
			IKH[i][j] = 0;
			for (int k = 0; k < STATE_DIM; ++k) {
				IKH[i][j] += (i == k ? 1.0 : 0.0) - K[i][k] * H[k][j];
			}
		}
	}

	// 然后计算 (I - KH)P_predicted  
	for (int i = 0; i < STATE_DIM; ++i) {
		for (int j = 0; j < STATE_DIM; ++j) {
			P[i][j] = 0;
			for (int k = 0; k < STATE_DIM; ++k) {
				P[i][j] += IKH[i][k] * P_predicted[k][j];
			}
		}
	}
	*state = xUpdated;
}

int main() {
	// 初始化  
	StateVector initialState = { 0.0, 0.0 }; // 假设初始状态为(0, 0)  
	StateVector state = initialState; // 初始化状态向量  
	double dt = 0.1; // 时间步长  
	double processNoise = 0.01; // 过程噪声  
	double measurementNoise = 0.1; // 测量噪声  
	std::vector<cv::Point3f> objectPoints; // 3D关节点  
	// 添加第一个点 (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）


	// 模拟迭代  
	for (int i = 0; i < objectPoints.size(); ++i) {
		// 从 objectPoints 获取观测值
		ObjectPoint observationPoint;
		observationPoint.x = objectPoints[i].x;
		observationPoint.y = objectPoints[i].y;
		// 假设我们每轮迭代使用一个不同的观测点
		//StateVector observation = observationFunction(observationPoint);
		// 执行扩展卡尔曼滤波
		extendedKalmanFilter(&state, dt, processNoise, measurementNoise, observationPoint);

		// 打印或处理更新后的状态
		printf("Iteration %d: State x = %f, y = %f\n", i + 1, state.x, state.y);
	}

	return 0;
}