无人机底层开发-MPU6050卡尔曼滤波的简单理解
卡尔曼滤波器由一系列递归数学公式描述。它们提供了一种高效可计算的方法来估计过程的状态，并使估计均方差最小。卡尔曼滤波器应用广泛且功能强大：它可以估计信号的过去和当前状态，甚至能估计将来的状态，即使并不知道模型的确切性质。
从本质上来讲，滤波就是一个信号处理与变换（去除或减弱不想要的成分，增强所需成分）的过程，这个过程既可以通过硬件来实现，也可以通过软件来实现。卡尔曼滤波属于一种软件滤波方法，其基本思想是：以最小均方差为最佳估计准则，采用信号与噪声的状态空间模型，利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值，算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方差的估计。
举一个简单的例子，我们通过温度计测得房间的温度，根据经验，室内的温度是恒定的，即上一分钟的温度等于现在的温度（假设我们用一分钟来做时间单位），但是毕竟经验不是很准，上下会有误差，我们把这个误差看成高斯白噪声。另外我们在房间放的温度计也不是非常准确，测量值会比实际值偏差，我们也把这些偏差看成是高斯白噪声。现在我们有了两个关于该房间的温度值：根据经验的预测值和温度计的值（测量值），下面根据这两个值并结合他们各自的噪声来估算出房间的实际温度值。
假如我们要估算k时刻的是实际温度值，首先要根据k-1时刻的温度值，来预测k时刻的温度。假设是k-1时刻是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定 度是4度，他们平方相加再开方，就是5）。然后k时刻从温度计读出的值是25度。由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance来判断。因为Kg^2=5^2/(5^2+4^2)，所以Kg=0.78，我们可以估算出k时刻的实际温度值 是：23+0.78*(25-23)=24.56度。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。
现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.56度）的偏差。算法如下：((1- Kg)*5^2)^0.5=2.35。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。就是这样，卡尔曼滤波器就不断的把covariance递归，从而估算出最优的温度值。他运行的很快，而且它只保留了上一时刻的covariance。上面的Kg，就是卡尔曼增益（KalmanGain）。
卡尔曼滤波有5大黄金公式，如下所述：
- 1、预测方程 (状态预测方程)：X(k|k-1) = AX(k-1|k-1) + BU(k)
X(k|k-1) -------- k-1时刻估计k时刻的状态值
A --------- 系统参数，对于多模型系统，它为矩阵
X(k-1|k-1) --------- k-1时刻的状态值
U(k) --- k时刻的状态控制量 (若无控制量的话，需要设定为0)
自己理解的方程：X(k) = a * X(k-1) ,在MPU6050中X(k)的值为陀螺仪测量的角速度
- 2、计算误差协方差 ：P(k|k-1) = AP(k-1|k-1)A' + Q
P(k|k-1) --------- k-1时刻估计k时刻的协方差
P(k-1|k-1) ------- k-1时刻的协方差
A' -------------- A的转置矩阵
Q ---------- 系统过程的过程噪声协方差
自己理解的方程：P(k) = A P(k-1) * A '
- 3、计算卡尔曼增益: Kg(k) = P(k|k-1)H' / (HP(k|k-1)H' + R)
Kg ------------ 卡尔曼增益
R ------------ 传感器的噪声平均值
自己理解的方程：Kg = P(k-1)/(P(k-1) + R)
- 4、修正估计 : X(k|k) = X(k|k-1) + Kg(k)(Z(k) - HX(k|k-1))
Z(k) ----------------- MPU6050测出的加速度计的值
自己理解的方程： X(k) = X(k-1) + Kg * (Z(k) - X(k-1))
X(k)即为陀螺仪测量的角速度
当Kg = 0时，状态值即为估计值
当Kg = 1时，状态值即为观测值
- 5、更新误差协方差：P(k|k) = (1 - Kg(k)H) P(k|k-1)
1 --- 矩阵，表示单模型单测量
自己理解的方程：P(k) = (1 - Kg ) * P(k - 1)
代码如下：
float angle_Y = 0; //初始状态值
float angle_X = 0;
float angle_Z = 0;
void Kalman_Y(Vector3i* acc,Vector3i* gyro)
{
float Q_angle = 0.001; // 陀螺仪噪声的协方差
float R_angle = 0.5; // 加速度计的协方差
float dt = 0.0002;
float Q_bias = 0; // Q_bias为陀螺仪漂移
float Q_bians = 0;
float k_0 =0,k_1 = 0; // 卡尔曼增益
float pdot[4] = {0,0,0,0};
float p[2][2] = {{1,0},{0,1}};
//1、卡尔曼第一个公式(状态预测):X(k|k-1)=AX(k-1|k-1)+BU(k)
angle_Y +=(gyro->y - Q_bias) * dt;
//2、卡尔曼第二个公式(计算误差协方差)：AP(k-1|k-1)A' + Q
pdot[0] = Q_angle - p[0][1] + p[1][0];
pdot[1] = -p[1][1];
pdot[2] = -p[1][1];
pdot[3] = Q_bias;
p[0][0] += pdot[0] * dt;
p[0][1] += pdot[1] * dt;
p[1][0] += pdot[2] * dt;
p[1][1] += pdot[3] * dt;
//3、卡尔曼第三个公式(计算卡尔曼增益):Kg(k) = P(k|k-1)H' / (HP(k|k-1)H' + R)
//R --->系统测量噪声协方差
k_0 = p[0][0]/(p[0][0]+R_angle);
k_1 = p[1][0]/(p[0][0]+R_angle);
//4、卡尔曼第四个公式(修正估计):X(k|k) = X(k|k-1) + Kg(k)(Z(k) - HX(k|k-1))
angle_Y += k_0 * ((atan2(-acc->x,acc->z) * RAD2DEG) - angle_Y);
Q_bians += k_1 * ((atan2(-acc->x,acc->z) * RAD2DEG) - angle_Y);
//5、卡尔曼第五个公式(更新误差协方差)：P(k|k) = (1 - Kg(k)H) P(k|k-1)
p[0][0] = (1 - k_0) * p[0][0];
p[0][1] = (1 - k_0) * p[0][1];
p[1][0] = (1 - k_0) * p[1][0];
p[1][1] = (1 - k_0) * p[1][1];
}
void Kalman_X(Vector3i* acc,Vector3i* gyro)
{
float Q_angle = 0.001; //陀螺仪噪声的协方差
float R_angle = 0.5; //加速度计的协方差
float dt = 0.0002;
float Q_bias = 0; //Q_bias为陀螺仪漂移
float Q_bians = 0;
float k_0 =0,k_1 = 0; //卡尔曼增益
float pdot[4] = {0,0,0,0};
float p[2][2] = {{1,0},{0,1}};
//1、卡尔曼第一个公式(状态预测):X(k|k-1)=AX(k-1|k-1)+BU(k)
angle_X +=(gyro->x - Q_bias) * dt;
//2、卡尔曼第二个公式(计算误差协方差)：AP(k-1|k-1)A' + Q
pdot[0] = Q_angle - p[0][1] + p[1][0];
pdot[1] = -p[1][1];
pdot[2] = -p[1][1];
pdot[3] = Q_bias;
p[0][0] += pdot[0] * dt;
p[0][1] += pdot[1] * dt;
p[1][0] += pdot[2] * dt;
p[1][1] += pdot[3] * dt;
//3、卡尔曼第三个公式(计算卡尔曼增益):Kg(k) = P(k|k-1)H' / (HP(k|k-1)H' + R)
//R --->系统测量噪声协方差
k_0 = p[0][0]/(p[0][0]+R_angle);
k_1 = p[1][0]/(p[0][0]+R_angle);
//4、卡尔曼第四个公式(修正估计):X(k|k) = X(k|k-1) + Kg(k)(Z(k) - HX(k|k-1))
angle_X += k_0 * ((atan2(acc->y,acc->z) * RAD2DEG) - angle_X);
Q_bians += k_1 * ((atan2(acc->y,acc->z) * RAD2DEG) - angle_X);
//5、卡尔曼第五个公式(更新误差协方差)：P(k|k) = (1 - Kg(k)H) P(k|k-1)
p[0][0] = (1 - k_0) * p[0][0];
p[0][1] = (1 - k_0) * p[0][1];
p[1][0] = (1 - k_0) * p[1][0];
p[1][1] = (1 - k_0) * p[1][1];
}
void Kalman_Z(Vector3i* mag,Vector3i* gyro)
{
float Q_angle = 0.001; //陀螺仪噪声的协方差
float R_angle = 0.5; //加速度计的协方差
float dt = 0.0002;
float Q_bias = 0; //Q_bias为陀螺仪漂移
float Q_bians = 0;
float k_0 =0,k_1 = 0; //卡尔曼增益
float pdot[4] = {0,0,0,0};
float p[2][2] = {{1,0},{0,1}};
//1、卡尔曼第一个公式(状态预测):X(k|k-1)=AX(k-1|k-1)+BU(k)
angle_Z +=(gyro->z - Q_bias) * dt;
//2、卡尔曼第二个公式(计算误差协方差)：AP(k-1|k-1)A' + Q
pdot[0] = Q_angle - p[0][1] + p[1][0];
pdot[1] = -p[1][1];
pdot[2] = -p[1][1];
pdot[3] = Q_bias;
p[0][0] += pdot[0] * dt;
p[0][1] += pdot[1] * dt;
p[1][0] += pdot[2] * dt;
p[1][1] += pdot[3] * dt;
//3、卡尔曼第三个公式(计算卡尔曼增益):Kg(k) = P(k|k-1)H' / (HP(k|k-1)H' + R)
//R --->系统测量噪声协方差
k_0 = p[0][0]/(p[0][0]+R_angle);
k_1 = p[1][0]/(p[0][0]+R_angle);
//4、卡尔曼第四个公式(修正估计):X(k|k) = X(k|k-1) + Kg(k)(Z(k) - HX(k|k-1))
angle_Z += k_0 * ((atan2(mag->x,mag->y) * RAD2DEG) - angle_Z);
Q_bians += k_1 * ((atan2(mag->x,mag->y) * RAD2DEG) - angle_Z);
//5、卡尔曼第五个公式(更新误差协方差)：P(k|k) = (1 - Kg(k)H) P(k|k-1)
p[0][0] = (1 - k_0) * p[0][0];
p[0][1] = (1 - k_0) * p[0][1];
p[1][0] = (1 - k_0) * p[1][0];
p[1][1] = (1 - k_0) * p[1][1];
}
运算宏定义如下：
#define MM2M 0.001f
#define DEG2RAD 0.017453292519943295769236907684886f // 角度转弧度
#define RAD2DEG 57.295779513082320876798154814105f // 弧度转角度
#define sq(a) ((a) * (a)) //平方、
#define sqrtf4(a,b,c,d) __sqrtf(sq(a) + sq(b) + sq(c) + sq(d))
#define sqrtf3(a,b,c) __sqrtf(sq(a) + sq(b) + sq(c))
#define sqrtf2(a,b) __sqrtf(sq(a) + sq(b))以上代码只为测滚转角，俯仰角和偏航角和它类似。
参考博文：卡尔曼滤波的原理（通俗易懂）：http://mini.eastday.com/bdmip/180326002020911.html?tdsourcetag=s_pcqq_aiomsg
卡尔曼滤波原理快速理解：https://blog.csdn.net/weixin_39449570/article/details/78846690