From a70319ae74f7517c519dbe0191d61cb55f9059da Mon Sep 17 00:00:00 2001 From: YcHepth Date: Tue, 27 Dec 2016 20:05:48 +0800 Subject: [PATCH] Code IN C MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 依赖于卡尔曼滤波算法演算。 --- filter.c | 44 +++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 43 insertions(+), 1 deletion(-) diff --git a/filter.c b/filter.c index 1fea560..2ac2cba 100644 --- a/filter.c +++ b/filter.c @@ -24,7 +24,49 @@ static float q_bias , angle_err , PCt_0 , PCt_1 , E , K_0 , K_1 , t_0 , t_1; Pdot 角速度协方差矩阵 dt 微分因子 q_bias 陀螺仪偏移量 - + C0 测量矩阵 + K0 K1 卡尔曼增益矩阵(2 * 1)元素 + PCt_0 PCt_1 t_0 t_1 中间计算变量 + angle_err 过程测量误差值 void kalman_fliter(float angle_m, float gyro_m, float *angle_f , *angle_dot_f) */ +void kalman_filter(float angle_m, float gyro_m, float *angle_f, float *angle_dot_f) +{ + angle += (gyro_m - q_bias) * dt; + + Pdot[0] = Q_angle - P[0][1] - P[1][0]; + Pdot[1] = -P[1][1]; + Pdot[2] = -P[1][1]; + Pdot[3] = Q_gyro; + + P[0][0] += Pdot[0] * dt; + P[0][1] += Pdot[1] * dt; + P[1][0] += Pdot[2] * dt; + P[1][1] += Pdot[3] * dt; + + angle_err = angle_m - angle; + + PCt_0 = C_0 * P[0][0]; + PCt_1 = C_0 * P[1][0]; + + E = R_angle + C_0 * PCt_0; + + K_0 = PCt_0 / E; + K_1 = PCt_1 / E; + + t_0 = PCt_0; + t_1 = C_0 * P[0][1]; + + P[0][0] -= K_0 * t_0; + P[0][1] -= K_0 * t_1; + P[1][0] -= K_1 * t_0; + P[1][1] -= K_1 * t_1; + + angle += K_0 * angle_err; + q_bias += K_1 * angle_err; + angle_dot = gyro_m - q_bias; + + *angle_f = angle; + *angle_dot_f = angle_dot; +}