kalman_filter_code - yuhannah/skills_map GitHub Wiki
KF卡尔曼滤波
代码实例：KF融合imu和odo的yaw和角速度wz
-
KF: ($\mu_{t-1},\sum_{t-1},u_t,z_t$)
$2. \overline{\mu}t=A_t\mu{t-1}+B_tu_t $--状态转移矩阵，预测值
$ 3. \overline{\sum}t=A_t\sum\nolimits{t-1}A_t^T+R_t $
$ 4. K_t=\overline{\sum}_tC_t^T(C_t \overline{\sum}_tC_t^T+Q_t)^{-1} $--卡尔曼增益
$ 5. \mu_t=\overline{\mu}_t+K_t(z_t-C_t\overline{\mu}_t) $
$ 6. \sum \nolimits{_t}=(I-K_tC_t)\overline{\sum}{_t} $
return $ \mu_t,\sum \nolimits{_t} $
imu: 50Hz, odo: 50Hz
参与KF融合的状态量是$ (w_z,yaw) $，二维状态；没有控制量；预测来自imu，观测来自里程计。
u8 yaw_fusion_new(imu_elements *_down_sample_del_imu, T_odo *_pro_odo, T_sensor_valid_flag *_sensor_valid_flag) {
float timediff = _down_sample_del_imu->delta_dt;
if (_down_sample_del_imu->delta_dt <= 0.0f) return yaw;
float wz_cov = 1.0e-5f;//z轴角速度的方差
float odo_R[2] = {1.0f, 1.0f};//里程计的方差
float SK[2] = {0.0f, 0.0f};//更新卡尔曼增益的中间量
float Kg[2][2];//卡尔曼增益
float odo_Z[2] = {0.0f, 0.0f};//里程计的观测值，（wz, yaw）
float predict_wz = 0.0f;//预测wz，来自imu
float predict_yaw = 0.0f;//预测yaw，来自imu和上次的融合结果
float predict_P[2][2] = {1.0f, 0.0f, 0.0f, 1.0f};//预测方差
static float P[2][2] = {1.0f, 0.0f, 0.0f, 1.0f};//卡尔曼滤波后的方差
static float wz = 0.0f;//卡尔曼滤波后的z轴角速度
predict_wz = _down_sample_del_imu->delta_ang.z / _down_sample_del_imu->delta_dt;//预测的角速度wz
predict_yaw = fusion_yaw + wz * _down_sample_del_imu->delta_dt;//预测的yaw
$2. \overline{\mu}t=A_t\mu{t-1}+B_tu_t $--状态转移矩阵，预测值
$ predict_w_z=\frac{\Delta yaw}{\Delta t} $
$ predict_yaw=fusion_yaw+w_z \cdot \Delta t $
$ w_z $是t-1时刻的角速度，$fusion_yaw$是t-1时刻的$yaw$。
if (1) {
predict_P[0][0] = P[0][0] + wz_cov;
predict_P[0][1] = P[0][1] + P[0][0] * timediff + wz_cov * timediff;
predict_P[1][0] = P[0][1] + P[0][0] * timediff + wz_cov * timediff;
predict_P[1][1] = P[1][1] + P[0][1] * timediff + wz_cov * timediff * timediff + P[1][0] * timediff + P[0][0] * timediff * timediff;
$ 3. \overline{\sum}t=A_t\sum\nolimits{t-1}A_t^T+R_t $--预测方差 $$ predict_P= \begin{bmatrix} 1 & 0 \ \Delta t & 1 \end{bmatrix} \begin{bmatrix} P_{00}+wz_cov & P_{01} \ P_{10} & P_{11} \end{bmatrix} \begin{bmatrix} 1 & \Delta t \ 0 & 1 \end{bmatrix} \ = \begin{bmatrix} P_{00}+wz_cov & P_{01} \ (P_{00}+wz_cov)\Delta t+P_{10} & P_{01}\Delta t +P_{11} \ \end{bmatrix} \begin{bmatrix} 1 & \Delta t \ 0 & 1 \end{bmatrix} \ = \begin{bmatrix} P_{00}+wz_cov & (P_{00}+wz_cov)\Delta t+P_{01} \ (P_{00}+wz_cov)\Delta t+P_{10} & ((P_{00}+wz_cov)\Delta t+P_{10})\Delta t+ P_{01}\Delta t +P_{11} \ \end{bmatrix} $$ 计算观测值里程计增量$z_t$，此处试图用反向里程计增量补偿imu增量，注意，正常的计算为(odo_r-odo_l)*K。
float delta_yaw = (_pro_odo->delta_odo_l - _pro_odo->delta_odo_r) * 2.0f * M_PI * WHEEL_RADIUS / PULSE_PER_CIRCLE / WHEEL_BASE;//！！！
if (fabs(delta_yaw) > 0.03f) {
wz = _down_sample_del_imu->delta_ang.z / _down_sample_del_imu->delta_dt;
fusion_yaw = predict_yaw;
if (fusion_yaw < -PI) {
fusion_yaw += 2 * PI;
} else if (fusion_yaw > PI) {
fusion_yaw -= 2 * PI;
}
yaw = fusion_yaw;
return 0;
}
odo_Z[0] = delta_yaw / _down_sample_del_imu->delta_dt;//！！！
odo_Z[1] = fusion_yaw + delta_yaw; //注意imu和odo分别累积的航向是不同的！
if (odo_Z[1] < -PI) {
odo_Z[1] += 2 * PI;
} else if (odo_Z[1] > PI) {
odo_Z[1] -= 2 * PI;
}
考虑打滑或者里程计数据无效，则使用imu数据。
if (_sensor_valid_flag->odo_valid == 0 ||
_sensor_valid_flag->odo_slip == 1)
{
wz = _down_sample_del_imu->delta_ang.z / _down_sample_del_imu->delta_dt;
fusion_yaw = predict_yaw;
if (fusion_yaw < -PI) {
fusion_yaw += 2 * PI;
} else if (fusion_yaw > PI) {
fusion_yaw -= 2 * PI;
}
yaw = fusion_yaw;
return 0;
}
$ 4. K_t=\overline{\sum}_tC_t^T(C_t \overline{\sum}_tC_t^T+Q_t)^{-1} $--卡尔曼增益
$SK=\frac{1}{predict_P+odo_R}$中间系数
$Kg=predict_P \cdot SK$卡尔曼增益系数
$Q_t=odo_R$
按照公式来准确推算增益系数，如下：(用P暂时代替predict_P简化) $$ (predict_P+odo_R)^{-1}=( \begin{bmatrix} P_{00} & P_{01} \ P_{10} & P_{11} \end{bmatrix}+ \begin{bmatrix} odo_R[0] & 0 \ 0 & odo_R[1] \end{bmatrix} )^{-1} \ = \begin{bmatrix} \frac{-(P_{11}+odo_R[1])}{P_{01}P_{10}-(P_{00}+odo_R[0])(P_{11}+odo_R[1])} & \frac{P_{01}}{P_{01}P_{10}-(P_{00}+odo_R[0])(P_{11}+odo_R[1])} \ \frac{P_{10}}{P_{01}P_{10}-(P_{00}+odo_R[0])(P_{11}+odo_R[1])} & \frac{-(P_{00}+odo_R[0])}{P_{01}P_{10}-(P_{00}+odo_R[0])(P_{11}+odo_R[1])} \end{bmatrix} $$
$$ predict_P(predict_P+odo_R)^{-1}= \ \begin{bmatrix} P_{00} & P_{01} \ P_{10} & P_{11} \end{bmatrix} \begin{bmatrix} \frac{-(P_{11}+odo_R[1])}{P_{01}P_{10}-(P_{00}+odo_R[0])(P_{11}+odo_R[1])} & \frac{P_{01}}{P_{01}P_{10}-(P_{00}+odo_R[0])(P_{11}+odo_R[1])} \ \frac{P_{10}}{P_{01}P_{10}-(P_{00}+odo_R[0])(P_{11}+odo_R[1])} & \frac{-(P_{00}+odo_R[0])}{P_{01}P_{10}-(P_{00}+odo_R[0])(P_{11}+odo_R[1])} \end{bmatrix} \ = \begin{bmatrix} \frac{P_{01}P_{10}-P_{00}(P_{11}+odo_R[1])}{P_{01}P_{10}-(P_{00}+odo_R[0])(P_{11}+odo_R[1])} & \frac{P_{00}P_{01}-P_{01}(P_{00}+odo_R[0])}{P_{01}P_{10}-(P_{00}+odo_R[0])(P_{11}+odo_R[1])} \ \frac{P_{10}P_{11}-P_{10}(P_{11}+odo_R[1])}{P_{01}P_{10}-(P_{00}+odo_R[0])(P_{11}+odo_R[1])} & \frac{P_{01}P_{10}-P_{11}(P_{00}+odo_R[0])}{P_{01}P_{10}-(P_{00}+odo_R[0])(P_{11}+odo_R[1])} \end{bmatrix} $$
假设$P_{01}P_{10}$很小（非对角线元素，近似为0？？？），则以上两式依次简化为： $$ SK=(predict_P+odo_R)^{-1}=( \begin{bmatrix} P_{00} & P_{01} \ P_{10} & P_{11} \end{bmatrix}+ \begin{bmatrix} odo_R[0] & 0 \ 0 & odo_R[1] \end{bmatrix} )^{-1} \ = \begin{bmatrix} \frac{-(P_{11}+odo_R[1])}{P_{01}P_{10}-(P_{00}+odo_R[0])(P_{11}+odo_R[1])} & \frac{P_{01}}{P_{01}P_{10}-(P_{00}+odo_R[0])(P_{11}+odo_R[1])} \ \frac{P_{10}}{P_{01}P_{10}-(P_{00}+odo_R[0])(P_{11}+odo_R[1])} & \frac{-(P_{00}+odo_R[0])}{P_{01}P_{10}-(P_{00}+odo_R[0])(P_{11}+odo_R[1])} \end{bmatrix} \ \approx \begin{bmatrix} \frac{1}{P_{00}+odo_R[0]} & \frac{P_{01}}{P_{01}P_{10}-(P_{00}+odo_R[0])(P_{11}+odo_R[1])} \ \frac{P_{10}}{P_{01}P_{10}-(P_{00}+odo_R[0])(P_{11}+odo_R[1])} & \frac{1}{P_{11}+odo_R[1]} \end{bmatrix} \ = \begin{bmatrix} SK[0] & * \
- & SK[1] \end{bmatrix} $$
$$ K_g=predict_P(predict_P+odo_R)^{-1}= \ \begin{bmatrix} \frac{P_{00}}{SK[0]} & \frac{P_{01}}{SK[1]} \ \frac{P_{10}}{SK[0]} & \frac{P_{11}}{SK[1]} \end{bmatrix} \ = \begin{bmatrix} \frac{P_{00}}{P_{00}+odo_R[0]} & \frac{P_{01}}{P_{11}+odo_R[1]} \ \frac{P_{10}}{P_{00}+odo_R[0]} & \frac{P_{11}}{P_{11}+odo_R[1]} \end{bmatrix} $$
for (int index = 0; index < 2; index++) {
SK[index] = 1.0 / (double) (predict_P[index][index] + odo_R[index]);
for (uint8_t i = 0; i <= 1; i++) {
Kg[i][index] = predict_P[i][index] * SK[index];
}
}
$ 5. \mu_t=\overline{\mu}_t+K_t(z_t-C_t\overline{\mu}_t) $--更新t时刻的融合结果 $$ K_g(odo_Z-predict)= \begin{bmatrix} K_g[00] & K_g[01] \ K_g[10] & K_g[11] \end{bmatrix} \begin{bmatrix} odo_Z[0]-predict_w_z \ odo_Z[1]-predict_yaw \end{bmatrix} \ = \begin{bmatrix} K_g00+K_g01 \ K_g10+K_g11 \end{bmatrix} $$
float innov_wz = odo_Z[0] - predict_wz;
float innov_yaw = odo_Z[1] - predict_yaw;
if (innov_yaw < -PI) {
innov_yaw += 2 * PI;
} else if (innov_yaw > PI) {
innov_yaw -= 2 * PI;
}
wz = predict_wz + Kg[0][0] * innov_wz + Kg[0][1] * innov_yaw;
fusion_yaw = predict_yaw + Kg[1][0] * innov_wz + Kg[1][1] * innov_yaw;
// wz = predict_wz + Kg[0][0]*innov_wz;
// yaw = predict_yaw + Kg[1][1]*innov_yaw;
$ 6. \sum \nolimits{_t}=(I-K_tC_t)\overline{\sum}{_t} $--更新方差 $$ P=(I-K_g)predict_P \ = \begin{bmatrix} 1-K_g[00] & -K_g[01] \ -K_g[10] & 1-K_g[11] \end{bmatrix} \begin{bmatrix} P[00] & P[01] \ P[10] & P[11] \end{bmatrix} \ = \begin{bmatrix} (1-K_g[00])P[00]-K_g[01]P[10] & (1-K_g[00])P[01]-K_g[01]P[11] \ -K_g[10]P[00]+(1-K_g[11])P[10] & -K_g[10]P[01]+(1-K_g[11])P[11] \end{bmatrix} $$ 根据方差对称性，P[01]=P[10]=(P[01]+P[10])/2
P[0][0] = (1.0f - Kg[0][0]) * P[0][0] - Kg[0][1] * P[1][0];
P[0][1] = (1.0f - Kg[0][0]) * P[0][1] - Kg[0][1] * P[1][1];
P[1][0] = -Kg[1][0] * P[0][0] + (1.0f - Kg[1][1]) * P[1][0];
P[1][1] = -Kg[1][0] * P[0][1] + (1.0f - Kg[1][1]) * P[1][1];
P[0][1] = 0.5f * (P[0][1] + P[1][0]);
P[1][0] = P[0][1];
if (fabs(P[0][0]) > 1.0e5f || fabs(P[1][1]) > 1.0e3f) {
wz = _down_sample_del_imu->delta_ang.z / _down_sample_del_imu->delta_dt;
fusion_yaw = predict_yaw;
P[0][0] = 1.0f;
P[0][1] = 0.0f;
P[1][0] = 0.0f;
P[1][1] = 1.0f;
}
if (fusion_yaw < -PI) {
fusion_yaw += 2 * PI;
} else if (fusion_yaw > PI) {
fusion_yaw -= 2 * PI;
}
最后，如果不进行融合，全部采用imu的数据，如下：
} else {//没有融合，累加yaw
wz = _down_sample_del_imu->delta_ang.z / _down_sample_del_imu->delta_dt;
fusion_yaw = predict_yaw;
if (fusion_yaw < -PI) {
fusion_yaw += 2 * PI;
} else if (fusion_yaw > PI) {
fusion_yaw -= 2 * PI;
}
}
yaw = fusion_yaw;
return 1;
}