卡爾曼濾波代碼問題
這是黑馬板大的卡爾曼濾波代碼：有些地方不理解想請問各位
13.#define Q_angle 0.01 // 角度数据置信度
14.#define Q_omega 0.0003 // 角速度数据置信度
15.#define R_angle 0.01 // 方差噪声
請問這三個值是怎麼得的啊??
47.// 卡尔曼滤波
48. Klm_angle += (omega - bias) * dt; // 先验估计
49. P_00 += -(P_10 + P_01) * dt + Q_angle *dt;
50. P_01 += -P_11 * dt;
51. P_10 += -P_11 * dt;
52. P_11 += +Q_omega * dt; // 先验估计误差协方差
53.
54. float K_0 = P_00 / (P_00 + R_angle);
55. float K_1 = P_10 / (P_00 + R_angle);
56.
57. bias += K_1 * (angleA - Klm_angle);
58. Klm_angle += K_0 * (angleA - Klm_angle); // 后验估计
59. P_00 -= K_0 * P_00;
60. P_01 -= K_0 * P_01;
61. P_10 -= K_1 * P_00;
62. P_11 -= K_1 * P_01; // 后验估计误差协方差
63.
64. Serial.print(timer);
65. Serial.print(",");
66. Serial.print(angleA, 6);
67. Serial.print(",");
68. Serial.print(angleG, 6);
69. Serial.print(",");
70. Serial.print(Com_angle, 6);
71. Serial.print(",");
72. Serial.print(Com2_angle, 6);
73. Serial.print(",");
74. Serial.print(Klm_angle, 6);
75. Serial.print(";"); // 输出数据
76. delay(50);
77.}
13.#define Q_angle 0.01 // 角度数据置信度
14.#define Q_omega 0.0003 // 角速度数据置信度
15.#define R_angle 0.01 // 方差噪声
請問這三個值是怎麼得的啊??
請問有人知道嗎?????
拜託幫個忙吧!!!!{:soso_e154:} 好帖不能沉啊~!!!拜託大家了:Q
页:
[1]