找到了一篇文章，这篇文章中的代码没有调用opencv的Eigen库，我使用这个库就会报错，在此代码基础上填充了自己的内容，使之运行。

百度安全验证

先贴一下代码作为记录，不然等我放进自己的程序里面就要面目全非了。

#include <stdio.h>
#include <math.h>

// 状态向量维度
#define STATE_DIM 2


// 状态向量
typedef struct {
	double x;
	double y;
} StateVector;     //定义了一个名为StateVector的类型，它包含两个双精度浮点数x和y

// 预测步骤中的非线性状态转移函数
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(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) {
	
	// 状态方程的雅可比矩阵
	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 z = observationFunction(xPredicted); // 假设observationFunction返回观测值  
	StateVector residual;
	residual.x = z.x - xPredicted.x;
	residual.y = z.y - xPredicted.y;

	// 更新状态估计  
	xUpdated.x = xPredicted.x + residual.x * K[0][0] + residual.y * K[0][1];
	xUpdated.y = xPredicted.y + residual.x * K[1][0] + residual.y * K[1][1];
	// 更新协方差矩阵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; // 测量噪声  
	extendedKalmanFilter(&state, dt, processNoise, measurementNoise);
	int iterations = 10;

	// 模拟迭代  
	for (int i = 0; i < iterations; ++i) {
		// 这里假设我们有一个观测值，但实际上可能需要根据实际传感器数据来设置  
		// 假设观测值就是当前状态（这里仅为了示例）  
		StateVector observation = stateTransition(state, dt);

		// 执行扩展卡尔曼滤波  
		extendedKalmanFilter(&state, dt, processNoise, measurementNoise);

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

		// （可选）这里可以添加根据观测值更新state的代码，但在这个例子中我们假设观测值就是真实状态  
	}

	return 0;
}

 贴一下运行效果，刚接触扩展卡尔曼滤波也不是很清楚这个运行结果咋样。是否符合要求。先酱吧，不合适又说吧。

Iteration 1: State x = 0.200000, y = 0.009983
Iteration 2: State x = 0.299995, y = 0.029850
Iteration 3: State x = 0.399950, y = 0.059402
Iteration 4: State x = 0.499774, y = 0.098339
Iteration 5: State x = 0.599291, y = 0.146262
Iteration 6: State x = 0.698223, y = 0.202668
Iteration 7: State x = 0.796177, y = 0.266953
Iteration 8: State x = 0.892634, y = 0.338422
Iteration 9: State x = 0.986962, y = 0.416295
Iteration 10: State x = 1.078422, y = 0.499730