卡尔曼滤波 公式

卡尔曼滤波公式
卡尔曼滤波是一种用于估计状态变量的数学算法，广泛应用于各种领域，如航空航天、无人驾驶、机器人等。

以下是卡尔曼滤波的公式：
1.状态预测方程：
x[k|k-1] = A[k|k-1] * x[k-1|k-1] + B[k|k-1] * u[k]
其中，x[k|k-1]表示在时间k对时间k-1的状态预测，A[k|k-1]是状态转移矩阵，B[k|k-1]是控制矩阵，u[k]是控制向量。

2.测量更新方程：
z[k|k] = H[k|k] * x[k|k] + v[k]
其中，z[k|k]表示在时间k对时间k的测量更新，H[k|k]是量测矩阵，v[k]是测量噪声。

3.协方差预测方程：
P[k|k-1] = A[k|k-1] * P[k-1|k-1] * A[k|k-1]' + Q
其中，P[k|k-1]表示在时间k对时间k-1的协方差预测，Q是过程噪声协方差。

4.协方差更新方程：
P[k|k] = (I - K[k] * H[k|k]) * P[k|k-1]
其中，P[k|k]表示在时间k对时间k的协方差更新，K[k]是卡尔曼增益矩阵。

5.卡尔曼增益计算：
K[k] = P[k|k-1] * H[k|k]' / (H[k|k] * P[k|k-1] * H[k|k]' + R)
其中，R是测量噪声协方差。