加速度计陀螺仪Kalman滤波-建模及C代码

11mod:1_*___010_:10__Pr:*_*_kkkkkkkSystemelangleangledtgyromdtwangleqbiasqbiaswgyromeasurementangleanglevangleqbiasocessNoisedtQangledtQgyroMeasurementNoise1k|k-1:_Pr:()_*_**0001P=1011100011*_0011011010*_kRangleocessUpdateangleangleqbiasdtgyromdtangleRatedtPPPPdtPPdtdtQanglePPdtQgKalmanFilter红色角度预测：偏差预测：（就是它自己，不做运算）方差预测：200(_0110)11*()01(11)*1011*11*_:__0000110001110_11011010110yroPdtQanglePPPdtPPdtPPdtPdtQgyroMeasurementUpdateangleerrincAngleangleKPPPPKPPPP新息：增益(深蓝色)：1k|kk|k-1_00/(00_)100001_0P==I-10P1011_10001_000001=-1011_10101100-_0*0001RanglePPRanglePPPKPPKPPKPPPPKPPPKPP协方差更新（紫色）：（预测方差和方差可以共用内存，减少内存浪费）_0*0110_0*1011_1*01_0=+*____1KPPKPPKPangleangleKangleerrqbiasqbiasK状态估计：//floatgyro_m：陀螺仪测得的量（角速度）//floatincAngle:加速度计测得的角度值#definedt0.0015//卡尔曼滤波采样间隔#defineR_angle0.69//测量噪声的协方差#defineQ_angle0.0001//角度噪声方差#defineQ_gyro0.0003//陀螺仪噪声方差floatkalmanUpdate(constfloatgyro_m,constfloatincAngle){floatK_0;//角度滤波增益floatK_1;//偏差滤波增益floatY_0;floatY_1；floatRate;//去除偏差后的角速度floatPdot[4];//计算状态预测协方差矩阵的中间矩阵floatangle_err;//角度偏量（新息）floatE;//计算的中间量staticfloatangle=0;//下时刻最优估计值角度staticfloatq_bias=0;//陀螺仪的偏差（状态分量）staticfloatP[2][2]={{1,0},{0,1}};//状态协方差矩阵Rate=gyro_m-q_bias;//KalmanTimeUpdate(状态更新)angle+=Rate*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;%%Innovation信息序列angle_err=incAngle-angle;//计算角度偏差E=R_angle+P[0][0];%%滤波增益K_0=P[0][0]/E;K_1=P[1][0]/E;//计算卡尔曼增益//更新新协方差矩阵Y_0=P[0][0];Y_1=P[0][1];P[0][0]-=K_0*Y_0;P[0][1]-=K_0*Y_1;P[1][0]-=K_1*Y_0;P[1][1]-=K_1*Y_1;angle+=K_0*angle_err;//给出最优估计值q_bias+=K_1*angle_err;//给出最优估计值偏差returnangle;}