칼만 필터 알고리즘의 5가지 핵심 공식의 의미
SLAM에서 자주 사용되는 칼만 필터 알고리즘, 다음은 후속 조회를 위한 칼만 필터 알고리즘의 5가지 핵심 공식에 대한 간략한 기록입니다. 공식의 파생은 반복되지 않으며, 칼만 필터 알고리즘의 5가지 핵심 공식은 직접 공개됩니다.
1. 상태 업데이트 방정식
xk ^ = F kx ^ k − 1 + B kuk \hat{x_k}={F_k}\hat{x}_{k-1}+{B_k}{u_k}엑스케이^=에프케이엑스^^케이 - 1+비케이유케이
P k = F k P k − 1 F k T + Q k {P_k}={F_k}{P_{k-1}}{F_k}^T+{Qk}피케이=에프케이피케이 - 1에프케이티+큐 케이
2. 측정 업데이트 방정식
x ^ ′ = xk ^ + K ( zk − H kxk ^ ) \hat{x}'=\hat{x_k}+K({z_k}-{H_k}\hat{x_k})엑스^^′′=엑스케이^+케이 ( 지케이-시간케이엑스케이^)
P k ′ = P k − KH k P k {P_k}'={P_k}-K{H_k}{P_k}피케이′′=피케이-K H케이피케이
K = P k H k T ( H k P k H k T + RK ) − 1 {K}={P_k}{H_k}^T ({H_k}{P_k}{H_k}^T+{R_K})^{-1}케이=피케이시간케이티 (H케이피케이시간케이티+아르 자형케이)− 1
Fk表示状态转移矩阵，描述系统的状态方程模型；
Bk表示状态的控制矩阵，描述系统的状态控制变化；
uk 为模型的修正向量，用于对建立模型的修正，该项在卡尔曼滤波算法中不是必备的；
Qk为过程噪声，描述了建立系统的模型准确度
Pk为协方差矩阵，描述了各状态量之间的相关性
P’k为经过修正的协方差矩阵
x^'k为经过量测方程修正的状态量估计值
K为卡尔曼增益，描述的是量测量对于状态量的修正权重
zk为观测量，多为传感器测量值或其等价值
Hk为量测矩阵，描述测量值与状态值之间的转换关系
Rk为量测噪声阵，描述传感器的测量噪声
整个算法的流程如下图所示：
流程的执行过程如下：
1.确定系统的状态转移矩阵Fk与量测矩阵Hk
2. 确定协方差矩阵初值P0与状态量初值 x0
3. 更新卡尔曼增益K
4. 根据测量向量zk 、卡尔曼增益K그리고 uk를 측정 하여 상태량을 수정하고 업데이트 주기의 상태 추정값 x^'k를
얻는다. 5. 공분산 행렬 P'k를
업데이트 한다. 6. 상태 전이 행렬인 재귀 상태 방정식에 따라 다음 주기 상태량 x^k
를 예측한다. 7. 상태 전이 행렬인 재귀 공분산 행렬에 따라 다음 주기 공분산 행렬 Pk를 예측한다.