卡尔曼滤波

标准卡尔曼滤波推导相关

预测(predict)

更新(update)

注意，以下对于时间的下标，有的时候用t有的时候用k，它们其实是一样的，因为参考不同的资料，所以写的比较乱。

其中是隐状态，也是我们想要获得的状态，但是我们无法测量，只能根据观测值进行推测，predict中的隐状态和update中的隐状态表示方式不一样，下面会详细讲述区别；

是观测值；

A是状态转移矩阵；

B是控制矩阵；

uk-1代表外部作用，应该是已知量；

代表的方差；

Q代表预测模型存在的噪声的协方差矩阵，个人理解应该是我们得到的uk-1已知量，和实际的uk-1存在偏差，这个偏差符合高斯分布；

H代表测量值y和隐状态x之间的转换矩阵，测量值y和隐状态x之间是线性关系的；

R是测量值y的测量噪声的协方差矩阵；

也是协方差矩阵对应update中的隐状态；

另外给出k时刻的隐状态和k-1时刻的隐状态的线性关系，以及观察值和隐状态之间的线性关系，如下：

这里的和predict和update中的不一样（predict和update中的带一个hat，小尖帽），这里的代表真实的隐状态，我们永远无法获取，实际工程使用中，只能选择概率密度函数中对应概率最大的值代表真实的隐状态，这里代表的就是k-1时刻预测模型存在的噪声，这个噪声服从高斯分布，协方差矩阵就是上述提到的Q（这里注意其实卡尔曼滤波实际使用的时候可以默认每一时刻的噪声的协方差矩阵是一个定值Q，Q可以通过训练模型获取，或者凭经验获取，同理，R也一样），代表k时刻观测值中包含的测量噪声，其对应的协方差矩阵为R。

===============================等号里面这部分参考https://www.bilibili.com/video/BV1BW411P7gV，基本不影响推导，看不懂可以忽略

所以根据这两个公式可以看出卡尔曼滤波和HMM模型类似（HMM模型的隐状态是离散的，卡尔曼滤波中的隐状态是连续的），都是马尔可夫链的一种，即

 或者   (此概率的英文名称：transition prob)  对应上述

当前隐状态只和上一时刻的隐状态有关，和再之前的状态没有关系;

 

  (此概率的英文名称：emmission prob / measurement prob)，对应上述

一旦知道当前的隐状态，则观测状态之间相互独立，并且只和当前隐状态有关。

==============================

下面我们回到最初的5个公式：

1）predict相当于根据以往1到t-1的观测值...来预测t时刻的隐状态，用概率公式来表示就是，由于隐状态是连续的，而不是离散的(discrete)，所以可以用概率密度函数表示，实际上这是一个高斯分布，，其中，是预测得到的此分布的期望，是预测得到的此分布的协方差矩阵，在上述预测的公式中代表的就是期望，代表的就是协方差矩阵，predict的第一个公式很好理解，就是根据来的，下面详细介绍第二个公式的由来：

的公式可以这么理解，模型噪声和和上次的隐状态是相互独立的，所以两者的协方差为0，即，噪声q的期望为0，而且是高斯分布，上述提到过，所以

其中是和方差一样的高斯分布，只不过期望不一样，期望为0，所以也成立。

所以：上述公式可以接着化简如下：

2）update我们可以理解为分两步实施：

a.根据测量值得到k时刻的隐状态值，此时我们就有了两个关于的数据，一个是根据实际测量值得到的，一个是根据上一个时刻的隐状态（这个隐状态是根据1到k-1的所有观测值得到的，所以也可以称作根据以往1到t-1的观测值...来预测t时刻的隐状态）预测当前时刻的隐状态，也就是predict中得到的，所以我们要将两者融合，得到更可靠的数据；

b.根据predict得，

根据测量值得到的得，，是常数，，所以

至此我们从不同途径得到两个关于的高斯分布，那我们应该相信哪个？实际的做法是两者融合，就是将两者的概率密度函数相乘，得到一个新的高斯分布，用新的高斯分布作为对隐状态的最优估计，其中我们肯定要考虑测量噪声和预测模型噪声，两个高斯噪声；

两个高斯分布相乘可以参考论文Products and Convolutions of Gaussian Probability Density Functions（若失效，有备份如下pdf备份）

这里直接说结论：

其中  和分别对应新高斯分布的期望和协方差矩阵。另外论文中只介绍了对两个一元高斯相乘的结果，没有介绍两个多元高斯的结果，直接就是多个多元高斯相乘的结果，其实两个多元高斯相乘的结果在形式上和两个一元高斯相乘是一样的。这里的形式也和论文中略有不同，是为了突出接下来要介绍的卡尔曼增益K；

将，两个分部的期望和方差分别代入得：

卡尔曼滤波中卡尔曼增益的值等于

所以第一条公式两边同乘H-1可得：

第二个公式（新高斯分布的期望）：

第三个公式（新高斯分布的方差）：

到此可以发现我们推导出来的公式和开头给的公式是一样的，个人感觉这里的证明并不严谨，因为H矩阵可能不是方阵无法求逆，但是最起码最终得到的形式是大致正确的，这种方式可以让我们很直观的理解，总结起来一句话 观测值和预测值融合，即可得到最优估计。

这里的证明方式是参考https://www.bilibili.com/video/BV15E411v7rD，但是不完全一致，区别在于高斯分布的概率密度函数相乘的时候，我是用隐状态的概率密度函数直接相乘，视频中是用观测值的概率密度函数相乘。如果完全照搬，我也不好意思说是原创的博客了，另外也是为了验证自己试图尝试的这种证明方法是否可行。

其实更为严谨的证明可以参考徐亦达老师的视频（https://www.bilibili.com/video/BV1TW411N7Hg），视频里不止介绍了卡尔曼滤波证明，还将卡尔曼滤波纳入线性高斯模型中，和HMM模型以及粒子滤波形成了一套完备的体系，建议观看，但是视频的最后结尾有点仓促，，的联合概率密度函数已经计算完成，conditional的概率密度函数没有没有完成，需要我们自己动手计算了，conditional的概率密度函数也就是本片博客中所提的最终两个高斯相乘的新的高斯的密度函数。

 

扩展卡尔曼滤波

标准卡尔曼滤波应用到六轴传感器上进行姿态计算还不行，因为它只适用于线性状况，所以引入扩展卡尔曼滤波（EKF），个人理解也就是由于非线性的原因，所以常量形式的状态转移矩阵和观测矩阵不再适用，转而使用随时间变化的雅可比矩阵。在下述公式中可以更直观的看到。

重点是，扩展卡尔曼滤波计算协方差矩阵P的时候不能直接使用标准卡尔曼滤波中的状态转移矩阵A，以及观测矩阵H，因为隐状态和上一时刻的隐状态xk-1不是线性的，观测值yk和隐状态也不是线性的，所以要引入雅可比矩阵F和H，公式如下（形式上是一样的，但内涵不一样了）：

预测(predict)

 

更新(update)

其中雅可比行列式和分别为：

，

可以看出雅可比矩阵是随时间变化的。

具体的推导过程可以参考网上的一篇博客https://blog.csdn.net/weixin_42647783/article/details/89054641

 

扩展卡尔曼滤波在姿态解算中的应用

先介绍姿态解算的基础知识，首先得了解四元数、欧拉角、旋转矩阵，这些知识和卡尔曼滤波是相互独立的知识，可以自行了解，这里直接给出要用的结论：

1、四元数微分方程

其中是四元数q关于时间的导数，上述乘积的结果是一个4x1的向量（矩阵），也可以称之为四元数q关于时间t的雅可比矩阵，由于时间t是标量，所以，此雅可比矩阵只有1列，四元数和每个时刻的空间姿态是一一对应的，可以转换成欧拉角或者旋转矩阵，就是六轴输出的原始角速度，根据四元数微分方程，更新四元数可以利用如下等式：

即：

整理得

也可以写成这样：

到这里我们容易犯一个错误，如果直接套用标准卡尔曼滤波的公式，那么

对应的就是状态转移矩阵A，然后将A用于接下来的协方差矩阵P的计算，这显然是错的，我们应该直接求雅可比矩阵Fk-1，而不是将上式写成标准卡尔曼滤波的形式，去拼凑转移矩阵A。

这里碰巧雅可比矩阵和拼凑出来的状态转移矩阵A相等，但是下面求雅可比矩阵Hk的时候就没有这种巧合了。只与为什么碰巧相等，其实很好理解：

 

2、雅可比矩阵Hk的求解

这里我们的观测值是重力加速度，重力加速度在世界坐标系下的坐标（右手系，东北天，分别对应x,y,z）Gw为(0, 0, -1)，写成四元数的形式为(0, 0, 0, -1)，单位化四元数表达的可以写成  

表达的含义是，为绕转轴右手定则方向旋转的角度，u为旋转轴。

所以单位化四元数(0, 0, 0, -1)的含义就是绕(0, 0, -1)旋转0度，也就是重力加速度在世界坐标系下的坐标的四元数形式。

重力加速度在机体坐标系下的坐标Gb为(0, x, y, z)，(x, y, z)实际上是加速度计的测量值，我们直接将其作为重力加速度，所以以下测量方程只适用于加速度计运动不剧烈的情况。

测量方程如下：

测量方程是根据四元数性质直接写的。

雅可比矩阵如下：

至此，状态方程，观测方程，以及雅可比矩阵和都已准备完毕，可以写代码了。

 

3、代码编写

首先有两点异样需要说明：

a.做此实验使用的平台是正点原子i.mx6ull开发板，六轴传感器为icm20608，使用spi接口读取数据，但是我发现开发板水平放置时，读取重力加速度的数值接近(0, 0, 1)，而不是公式里面的(0, 0, -1)，所以上述观测方程h(q)右侧应该乘以-1，雅可比矩阵Hk的每一项也应该乘以-1，这样世界坐标系的坐标轴不是指向东北天了，可能是西北地，未深究。

b.使用的上位机是匿名四轴，我并不知道欧拉角row pitch yaw的旋转顺序我也不知道，所以四元数到欧拉角的转化我也无法详细确定，是随便使用网上的一个公式（https://blog.csdn.net/waihekor/article/details/103748333），然后自行测试调整的，例如row的计算前面加了负号。

最终的效果是开发板转动和上位机转动是可以同步的，但是yaw会一直飘移，大概几秒钟1度，这个问题还未解决。

代码如下，首先是矩阵声明：

static float dt = SAMPLE_PERIOD * 0.001f;
/* 观测值，这里是重力加速度 */
static VEC * Z = NULL;
static VEC * Z_p = NULL;
static VEC * Z_e = NULL;
/* 状态转移方程的雅可比矩阵 */
static MAT * F = NULL;
static MAT * F_t = NULL;
/* 隐状态，这里是四元数 */
static VEC * q = NULL;
static VEC * q_p = NULL;
static VEC * q_e = NULL;

/* 观测方程的雅可比矩阵 */
static MAT * H = NULL;
static MAT * H_t = NULL;

/* 角速度计观测噪声协方差矩阵 */
static MAT * Q = NULL;
/* 加速度计观测噪声协方差矩阵 */
static MAT * R = NULL;

/* 观测值协方差矩阵 */
static MAT * P = NULL;
static MAT * P_tmp1 = NULL;
static MAT * P_tmp2 = NULL;
static MAT * P_p = NULL;

static MAT * K = NULL;
static MAT * K_tmp1 = NULL;
static MAT * K_tmp2 = NULL;
static MAT * K_tmp3 = NULL;

static MAT * I = NULL;

矩阵初始化函数：

void IMU_init(){
	Z = v_get(3);
	Z_p = v_get(3);
	Z_e = v_get(3);
	
	F = m_get(4, 4);
	F_t = m_get(4, 4);
	
	q = v_get(4);
	q_p = v_get(4);
	q_e = v_get(4);
	q->ve[0] = 0;
	q->ve[1] = 0;
	q->ve[2] = 0;
	q->ve[3] = 1;
	
	H = m_get(3, 4);
	H_t = m_get(4, 3);

	Q = m_get(4, 4);
	R = m_get(3, 3);
	m_zero(Q);
	m_zero(R);
	Q->me[0][0] = 1e-6;
	Q->me[1][1] = 1e-6;
	Q->me[2][2] = 1e-6;
	Q->me[3][3] = 1e-6;

	/* 加速度计噪声大，所以协方差矩阵数值取大点 */
	R->me[0][0] = 1e-1;
	R->me[1][1] = 1e-1;
	R->me[2][2] = 1e-1;

	P = m_get(4, 4);
	P_tmp1 = m_get(4, 4);
	P_tmp2 = m_get(4, 4);
	P_p = m_get(4, 4);
	m_zero(P);
	P->me[0][0] = 1;
	P->me[1][1] = 1;
	P->me[2][2] = 1;
	P->me[3][4] = 1;
	

	K = m_get(4, 3);
	K_tmp1 = m_get(4, 3);
	K_tmp2 = m_get(3, 4);
	K_tmp3 = m_get(3, 3);

	I = m_get(4, 4);
	m_zero(I);
	I->me[0][0] = 1;
	I->me[1][1] = 1;
	I->me[2][2] = 1;
	I->me[3][3] = 1;
}

卡尔曼滤波过程如下：

void IMU_Update(float gx, float gy, float gz, float ax, float ay, float az,
					float * roll, float * pitch, float * yaw){
	float norm;
	norm = sqrt(ax*ax + ay*ay + az*az);
	
	/* 加速度读取及单位化 */
	Z->ve[0] = ax/norm;
	Z->ve[1] = ay/norm;
	Z->ve[2] = az/norm;
	PRINT_VEC(Z);


	/* 雅克比矩阵F */	
	F->me[0][0] = 1; 		  F->me[0][1] = -gx*dt*0.5f; F->me[0][2] = -gy*dt*0.5f; F->me[0][3] = -gz*dt*0.5f;
	F->me[1][0] = gx*dt*0.5f; F->me[1][1] = 1;           F->me[1][2] =  gz*dt*0.5f; F->me[1][3] = -gy*dt*0.5f; 
	F->me[2][0] = gy*dt*0.5f; F->me[2][1] = -gz*dt*0.5f; F->me[2][2] = 1;           F->me[2][3] =  gx*dt*0.5f;
	F->me[3][0] = gz*dt*0.5f; F->me[3][1] =  gy*dt*0.5f; F->me[3][2] = -gx*dt*0.5f; F->me[3][3] = 1;
	PRINT_MAT(F);

	/* 状态转移方程的雅克比矩阵F的转置F_t */
	m_transp(F, F_t);
	PRINT_MAT(F_t);

	/* update */
	mv_mlt(F, q, q_p);
	/* predict四元数的单位化 */
	norm = v_norm2(q_p);
	q_p->ve[0] /= norm;
	q_p->ve[1] /= norm;
	q_p->ve[2] /= norm;
	q_p->ve[3] /= norm;
	PRINT_VEC(q_p);

	m_mlt(F, P, P_tmp1);
	m_mlt(P_tmp1, F_t, P_tmp2);
	m_add(P_tmp2, Q, P_p);/* 更新协方差矩阵P */
	PRINT_MAT(P_p);
	

	/* predict */
    /* 观测方程和上述理论分析中也差了-1 */
	#if 1
	Z_p->ve[0] = -2 * (q_p->ve[0]*q_p->ve[2] - q_p->ve[1]*q_p->ve[3]);
	Z_p->ve[1] = 2 * (q_p->ve[0]*q_p->ve[1] + q_p->ve[2]*q_p->ve[3]);
	Z_p->ve[2] = -1*(q_p->ve[1]*q_p->ve[1] + q_p->ve[2]*q_p->ve[2] - q_p->ve[0]*q_p->ve[0] - q_p->ve[3]*q_p->ve[3]);
	#else
	Z_p->ve[0] = 2 * (q_p->ve[0]*q_p->ve[2] - q_p->ve[1]*q_p->ve[3]);
	Z_p->ve[1] = -2 * (q_p->ve[0]*q_p->ve[1] + q_p->ve[2]*q_p->ve[3]);
	Z_p->ve[2] = q_p->ve[1]*q_p->ve[1] + q_p->ve[2]*q_p->ve[2] - q_p->ve[0]*q_p->ve[0] - q_p->ve[3]*q_p->ve[3];
	#endif
	norm = v_norm2(Z_p);
	Z_p->ve[0] /= norm;
	Z_p->ve[1] /= norm;
	Z_p->ve[2] /= norm;
	PRINT_VEC(Z_p);

	/* 观测方程的雅克比矩阵H */	
    /* 可以看到此矩阵和理论分析是的矩阵差了个-1 */
	H->me[0][0] = -2*q_p->ve[2]; H->me[0][1] =  2*q_p->ve[3]; H->me[0][2] = -2*q_p->ve[0]; H->me[0][3] =  2*q_p->ve[1];
	H->me[1][0] =  2*q_p->ve[1]; H->me[1][1] =  2*q_p->ve[0]; H->me[1][2] =  2*q_p->ve[3]; H->me[1][3] =  2*q_p->ve[2]; 
	H->me[2][0] =  2*q_p->ve[0]; H->me[2][1] = -2*q_p->ve[1]; H->me[2][2] = -2*q_p->ve[2]; H->me[2][3] =  2*q_p->ve[3];
	PRINT_MAT(H);
	m_transp(H, H_t);
	PRINT_MAT(H_t);

	m_mlt(P_p, H_t, K_tmp1);
	m_mlt(H, P_p, K_tmp2);
	m_mlt(K_tmp2, H_t, K_tmp3);
	m_add(K_tmp3, R, K_tmp3);
	m_inverse(K_tmp3, K_tmp3);
	m_mlt(K_tmp1, K_tmp3, K);
	PRINT_MAT(K);

	m_mlt(K, H, P_tmp1);
	m_sub(I, P_tmp1, P_tmp2);
	m_mlt(P_tmp2, P_p, P);
	PRINT_MAT(P);

	v_sub(Z, Z_p, Z_e);
	mv_mlt(K, Z_e, q_e);
	v_add(q_p, q_e, q);
	
	norm = v_norm2(q);
	q->ve[0] /= norm;
	q->ve[1] /= norm;
	q->ve[2] /= norm;
	q->ve[3] /= norm;
	PRINT_VEC(q);

#if 0
	*roll = atan2(2*q->ve[2]*q->ve[3]+2*q->ve[0]*q->ve[1], -2*q->ve[1]*q->ve[1]-2*q->ve[2]*q->ve[2] + 1)* 57.3;
	*pitch = asin(-2*q->ve[1]*q->ve[3]+2*q->ve[0]*q->ve[2])*57.3;
	*yaw = atan2(2*q->ve[1]*q->ve[2]+2*q->ve[0]*q->ve[3], q->ve[0]*q->ve[0] + q->ve[1]*q->ve[1] - q->ve[2]*q->ve[2] - q->ve[3]*q->ve[3]) * 57.3;

	
#endif
	*pitch = asin(2*q->ve[2]*q->ve[3]+2*q->ve[0]*q->ve[1])* 57.3;
    /* 这里和网络上的公式差了-1，要不然开发版和匿名四轴的roll是反的 */
	*roll = -atan2(2*q->ve[0]*q->ve[2]-2*q->ve[1]*q->ve[3], q->ve[0]*q->ve[0] - q->ve[1]*q->ve[1] - q->ve[2]*q->ve[2] + q->ve[3]*q->ve[3])* 57.3;
	*yaw = atan2(2*q->ve[0]*q->ve[3]-2*q->ve[1]*q->ve[2], q->ve[0]*q->ve[0] - q->ve[1]*q->ve[1] + q->ve[2]*q->ve[2] - q->ve[3]*q->ve[3])* 57.3;

}

和理论分析中的差别代码里都已表明。

另外使用上述代码之前，需要手动移植meschach，这是一个很老的矩阵库，C版本的，我也是网上随便收的，移植教程如下：https://www.cnblogs.com/maomao194313/p/3838362.html

注意移植的时候需要把Makefile里的gcc，ar等换成交叉编译器，上述教程不能算是移植，只能算是源码安装，这个矩阵库的使用手册如下：http://homepage.divms.uiowa.edu/~dstewart/meschach/html_manual/

另外上述代码的github地址如下：https://github.com/sallenkey-wei/I.MX6ull_test/tree/master/IMX6ULL/Drivers/Linux_Drivers/22_spi/kalman

视频演示如下：https://www.bilibili.com/video/BV1Fi4y1M7ju/