背景
利用手头的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滤波结果更加靠近直接由加速度计计算出的结果,反之则更加靠近陀螺仪积分得出的结果。可以做一些定性的分析和感性的理解(不准确的理解)
- V其实是测量的误差的反应,V越大表明误差越大,在经过数次迭代后,滤波结果中对观测值的“参考”就不是那么多。
- W则是整个模型递推过程中建模误差的反应,W越大,说明模型得出的结论越不可信,在经过数次迭代后,滤波结果中对模型推导值的“参考”就会下降。
下一步计划
- 理解PWV这三个量的含义,尝试给出定量的值。
- 与一个精确的航姿传感器进行对比。
- 加入磁力计进行计算