robot/KalmanFilter
1
0
Fork 0
mirror of https://github.com/ycHepth/KalmanFilter.git synced 2026-03-13 10:23:28 +08:00
Code Issues Packages Projects Releases Wiki Activity

Code IN C

Browse Source 
依赖于卡尔曼滤波算法演算。
This commit is contained in:
YcHepth
2016-12-27 20:05:48 +08:00
parent bf5bd9b5c0
commit a70319ae74
1 changed files with 43 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

44
filter.c
View File

@@ -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;
}