航姿传感器——扩展卡尔曼滤波（EKF）初步（一）

背景

利用手头的MPU6050传感器中的三轴陀螺仪及三轴加速度计实现一个基础的航姿传感器。阶段目标，VC++上实现EKF算法。
主要参考的是A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9-D IMU这篇论文。

代码

1、结构体初始化

• 卡尔曼滤波器结构体初始化。主要是几个变量的初始化，由于是EKF算法，所以多出了dH和Zkk_1这两个变量。EKF在进行kalman增益计算和协方差矩阵更新的时候，是近似看作普通的KF的，故这两个式子中的观测矩阵要求偏导数及原始观测阵的Jacobian Matrix，代码中用dH表示。而在残差（residual)或者说是新息的计算中应该是用 观测值-推测值，这里的推测值是用原始的观测阵推出的，代码中用Zkk_1表示。
• 可能有几个变量不涉及后续的计算。

typedef struct testKF {
	Eigen::VectorXd X;	// 状态向量	
	Eigen::MatrixXd PHI;// 状态转移矩阵
	Eigen::MatrixXd H;	// 观测矩阵
	Eigen::MatrixXd dH;	// 观测矩阵Jacobian Matrix
	Eigen::MatrixXd P;	// 状态协方差矩阵
	Eigen::MatrixXd W;	// 过程噪声矩阵Q
	Eigen::MatrixXd V;	// 观测噪声矩阵R
	Eigen::VectorXd Z;	// 观测向量
	Eigen::VectorXd Zkk_1;// 先验观测估计
	Eigen::MatrixXd K;	// kalman增益矩阵
	testKF() {
		X.resize(4);
		PHI.resize(4, 4);
		H.resize(3, 4);
		dH.resize(3, 4);
		P.resize(4, 4);
		W.resize(4, 4);
		V.resize(3, 3);
		Z.resize(3);
		Zkk_1.resize(3);
		K.resize(4, 3);
	}
}testKF;

2、kalman变量初始化

• 主要是 X,P,W,V这四个变量的初始赋值
• 具体赋值的多少可以参考论文中最后给出的几个数值，这里我稍微调节了下
• 后续会探究下P,W,V这三个矩阵该如何赋值，主要是背后的物理含义，数学含义到底是什么这是个关键！！

void ekf_init(testKF* kf)
{
	double q[4] = {1,0,0,0};
	//double w[3] = { 0, 0, 0};
	double acc[3] = { 0, 0, -1 };
	//初始化 X,P
	kf->X << q[0], q[1], q[2], q[3];
	//复制论文中的数值
	kf->P <<0.125, 0.0003, 0.0003, 0.0003,
			0.0003, 0.125, 0.0003, 0.0003,
			0.0003, 0.0003, 0.125, 0.0003,
			0.0003, 0.0003, 0.0003, 0.125;
	double DQ = 0.05*DEG2RAD*0.05*DEG2RAD;
	kf->W << DQ*Eigen::MatrixXd::Identity(4,4);
	kf->V << 0.05 * Eigen::MatrixXd::Identity(3, 3);
}

3、数值读取

• 这部分主要是读取原始数据
• 更新状态量X，观测矩阵dH，先验状态观测值Zkk_1

std::vector<std::vector<double>> ekf_test(std::vector<std::vector<double>>&data)
{
	std::vector<std::vector<double>> ekfout;
	testKF* kf = &QEKF;
	ekf_init(kf);
	size_t count = 0;
	for (auto line : data)
	{	
		double wx, wy, wz;
		wx = line[4]; wy = line[5]; wz = line[6];
		kf->Z << -line[7], -line[8], -line[9];
		// 状态转移阵
		double ts = S_IN_TIME;//采样间隔
		kf->PHI <<	0, -wx, -wy, -wz,
					wx, 0, wz, -wy,
					wy, -wz, 0, wx,
					wz, wy, -wx, 0;
		kf->PHI = Eigen::MatrixXd::Identity(4, 4) + kf->PHI*ts / 2;
		// 状态量
		Eigen::VectorXd Xkk_1(4);
		Xkk_1 = kf->PHI*kf->X;
		double q0, q1, q2, q3;
		q0 = Xkk_1[0]; q1 = Xkk_1[1]; q2 = Xkk_1[2]; q3 = Xkk_1[3];
		// 观测矩阵（Jabobain Matrix）
		kf->dH << -q2, q3, -q0, q1,
			q1, q0, q3, q2,
			q0, -q1, -q2, q3;
		kf->dH = 2 * kf->dH;
		// 先验观测估计
		kf->Zkk_1 << 2 * q1*q3 - 2 * q0*q2, 2 * q0*q1 + 2 * q2*q3, q0*q0 - q1 * q1 - q2 * q2 + q3 * q3;

		ekf_update(kf);

		double qx[4];
		qx[0] = kf->X[0]; qx[1] = kf->X[1]; qx[2] = kf->X[2]; qx[3] = kf->X[3];
		quaternions_norm(qx);
		kf->X[0] = qx[0]; kf->X[1] = qx[1]; kf->X[2] = qx[2]; kf->X[3] = qx[3];
		std::vector<double>vec = ekf_test_out(kf->X);
		double eul[3];
		Quaternions2Eular(qx,eul);
		vec.push_back(eul[0]);
		vec.push_back(eul[1]);
		vec.push_back(eul[2]);
		for (auto val : vec)
		{
			line.push_back(val);
			//std::cout << val;
		}
		ekfout.push_back(line);
	}
	return ekfout;
}

5、卡尔曼滤波更新

• 套用公式运算，没啥好说的

void ekf_update(testKF* kf)
{
	Eigen::VectorXd Xkk_1 = kf->PHI*kf->X;
	Eigen::MatrixXd Pkk_1 = kf->PHI*kf->P*kf->PHI.transpose() + kf->W;
	//EKF kalman增益计算时用H阵的Jacobian matrix
	kf->K = Pkk_1 * kf->dH.transpose()*((kf->dH*Pkk_1*kf->dH.transpose() + kf->V).inverse());
	kf->X = Xkk_1 + kf->K*(kf->Z - kf->Zkk_1);//EKF X更新时残差用 原始观测阵
	kf->P = (Eigen::MatrixXd::Identity(kf->K.rows(), kf->K.rows())-kf->K*kf->dH)*Pkk_1;
}

试验结果

滚转角：绿色加速度计计算，蓝色积分计算，红色EKF计算

俯仰角：绿色加速度计计算，蓝色积分计算，红色EKF计算

初步结论

• 可以看到当俯仰角的积分计算出现发散时，ekf计算结果保持收敛
• 相比加速度计得出的计算结果，EKF得出的结果要平滑许多。
• 尝试调节初始化中的W,V矩阵，当W（过程噪声矩阵）保持不变时，减小V（观测噪声矩阵）时，ekf滤波结果更加靠近直接由加速度计计算出的结果，反之则更加靠近陀螺仪积分得出的结果。可以做一些定性的分析和感性的理解（不准确的理解）
  1. V其实是测量的误差的反应，V越大表明误差越大，在经过数次迭代后，滤波结果中对观测值的“参考”就不是那么多。
  2. W则是整个模型递推过程中建模误差的反应，W越大，说明模型得出的结论越不可信，在经过数次迭代后，滤波结果中对模型推导值的“参考”就会下降。

下一步计划

• 理解PWV这三个量的含义，尝试给出定量的值。
• 与一个精确的航姿传感器进行对比。
• 加入磁力计进行计算