請問有關四軸卡爾曼濾波的問題!!

隨風大俠

請問卡爾慢濾波中的:

#define Q_angle 0.01      // 角度数据置信度
#define Q_omega 0.0003    // 角速度数据置信度
#define R_angle 0.01      // 方差噪声

這三個要如何決定???

這三個值會影響到哪些改變???

先說說我自己測試的結果:

我自己調了以後發現"角度数据置信度"調越大感測姿態的速度會越靈敏，但是對於抗震的效果就越差，調小則反之。


"角速度数据置信度" 這個值我調了後都沒什麼改變，不知是否有人也試過??

"方差噪声" 這個值調大後，感測器的值會超出原本的大小，然後慢慢減低。

好像有一種自動調整這三個值的方法，不知是否有人知道??




Klm_angle += (omega2 - bias) * dt;          // 先验估计
P_00 += -(P_10 + P_01) * dt + Q_angle *dt;
P_01 += -P_11 * dt;
P_10 += -P_11 * dt;
P_11 += +Q_omega * dt;                     // 先验估计误差协方差   
float K_0 = P_00 / (P_00 + R_angle);
float K_1 = P_10 / (P_00 + R_angle);
bias += K_1 * (angleA2 - Klm_angle);
Klm_angle += K_0 * (angleA2 - Klm_angle);   // 后验估计
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;


謝謝