-1]РР+РР3+Р3РgРb_РР<0Р0丿Рb_РР_0_РgyroР0_РРaРacceР=[1Р0]Р式中，®为包含固定偏差的陀螺仪输出角速度，CP为加速度计经处理后得到РgyroacceР的角度值，①为陀螺仪测量噪声，3为加速度传感器测量噪声，b为陀螺仪漂gaР移误差，①和3相互独立，此处假设二者为满足正态分布的白色噪声。令T为РgasР系统采样周期，得到离散系统的状态方程和测量方程：РРr1Р-T]РР~T~РР~3(k)T~РX(k)=Р<0РsРX(k-1)+РsР3(k—1)+РgsРР1JРР0РgyroР_0_РV(k)=[1РiР0]X(k)+3(k)РaРРРРР同时，要估算k时刻的实际角度，就必须根据k-1时刻的角度值，再根据预测得到的k时刻的角度值得到k时刻的高斯噪声的方差，在此基础之上卡尔曼滤波器进行递归运算直至估算出最优的角度值。在此，须知道系统过程噪声协方差阵Q以及测量误差的协方差矩阵R,对卡尔曼滤波器进行校正，Q与R矩阵的形式如下：Р(q0]РQ=_0cceR二[r]РISgyro丿-Р式中。q_acce和q_gyro分别是加速度传感器和陀螺仪测量的协方差，其数值代表卡尔曼Р滤波器对其传感器数据的信任程度，数值越小表明信任程度越高。在该系统中陀螺仪的值更Р为接近准确值，因此取q_gyro的值小于q_acce的值。当前状态：РX(kIk-1)二AX(k-11k-1)+BU(k)Р(1)Р4.4卡尔曼滤波化简РX-A*X+B*gyroР10РP=A*P*A'+QР10РKg=P*H/H*P*H'*+RР11РX=X+Kg*(acc-H*X)011РP=(I-Kg*H)*PР01Р上述五个公式便是卡尔曼滤波的五条数学公式在本项目中的使用。但是，以上五个公式,Р仅仅只是矩阵形式。虽然可以在MATLAB里进行仿真，但是却不能使用单片机进行有效运
全文预览
