卡尔曼滤波.txt

//在程序中利用Angle+=(Gyro - Q_bias) * dt计算出陀螺仪积分出的角度,其中Q_bias是陀螺仪偏差。 //此时利用陀螺仪积分求出的Angle相当于系统的估计值,得到系统的观测方程;而加速度计检测的角度Accel相当于系统中的测量值,得到系统状态方程。 //程序中Q_angle和Q_gyro分别表示系统对加速度计及陀螺仪的信任度。根据Pdot = A*P + P*A' + Q_angle计算出先验估计协方差的微分,用于将当前估计值进行线性化处理。其中A为雅克比矩阵。 //随后计算系统预测角度的协方差矩阵P。计算估计值Accel与预测值Angle间的误差Angle_e