0. 简介
作者之前对KF,EKF,UKF,PF都进行了学习，但是有两块KF还没有进行精细的学习，而相较于IEKF而言，ESKF会在滤波和融合定位中更常使用，当然学习了KF后，对于其他的变种卡尔曼滤波理解起来会非常容易，基本上问题不是很大。状态误差卡尔曼(ESKF)的应用，它是卡尔曼滤波器的变种中应用最为广泛的一种，与EKF一样，它也是一种针对时变系统的非线性滤波器。但是与EKF不同的是，它的线性化是总是在0附近，因此线性化更准确。绝大部分的场景，ESKF就足够使用了。如果对于滤波有更高的要求，可以选择UKF，甚至PF。
1. ESKF介绍
Error-state Kalman Filter（ESKF）就是一种传感器融合的算法，它的基础仍然是卡尔曼滤波。它的核心思想是把系统的状态分为三类：
- true state：实际状态，系统实际的运行状态
- nominal state：名义状态，描述了运动状态的主要趋势，主导成分。（large-signal，非线性）
- error state：误差状态，实际状态与名义状态之间的差值（small-signal，近似线性，适合线性高斯滤波）
基于以上状态分类，我们可以将关心的true-state，分为两部分，分别进行估计，即nominal state和error state，然后再进行二者叠加。 ESKF的全过程可以描述为如下步骤：
- 对高频率的IMU数据 进行积分，获得nomimal state 。nominal state里面不考虑噪声，因此势必会累积误差。这部分误差可依据error state进行修正。
- 利用Kalman Filter估计error state，这里同样也包括时间更新和量测更新两部分。这个过程考虑了噪声，并且由于误差状态方程是近似线性的，所以可以直接用卡尔曼滤波。
- 利用error state修正nominal state，获取true state。
- 重置error state
使用ESKF的优势：
- error state 中的参数数量与运动自由度是相等的，避免了过参数化（over-parameterization or redundancy）引起的协方差矩阵奇异的风险。
- error state 总是接近于0，Kalman Filter工作在原点附近。因此，远离奇异值、万向节锁，并且保证了线性化的合理性和有效性。
- error state 总是很小，因此二阶项都可以忽略，因此雅可比矩阵的计算会很简单，很迅速。
- error state 的变化平缓，因此KF修正的频率不需要太高。
1.1 Nominal State 构建
这部分只要是自更新，将IMU的所有状态量成分进行提取，并构建出一个Nominal state的动力学模型，且这个模型是不受噪声分量影响的，因为噪声分量的影响都放到了Error-state的动力学模型中。
其中 也就意味着
void ESKF_update_nominal(ImuDataPtr imu_data,double dt, State* state, Eigen::Vector3d g){
double dt2_2 = 0.5*dt*dt;
State old_state = *state;
state->G_p_I = old_state.G_p_I + dt * old_state.G_v_I + dt2_2 * (old_state.G_q_I * (imu_data->accel - old_state.ab) + g);
state->G_v_I = old_state.G_v_I + dt * (old_state.G_q_I * (imu_data->accel - old_state.ab) + g);
if (imu_data->quat.norm() != 0 && imu_data->last_quat.norm() != 0){
//quaternion valid, update orientation according to delta quaternion
Eigen::Matrix3d d_rot(imu_data->last_quat.inverse() * imu_data->quat);
state->G_q_I = old_state.G_q_I * d_rot;
const Eigen::Vector3d d_theta = (- old_state.wb) * dt;
if (d_theta.norm() >= 1e-12){
Eigen::AngleAxisd d_rot2(d_theta.norm(),d_theta.normalized());
state->G_q_I *= d_rot2.toRotationMatrix();
}
}else{
const Eigen::Vector3d d_theta = (imu_data->gyro - old_state.wb) * dt;
if (d_theta.norm() >= 1e-12){
Eigen::AngleAxisd d_rot(d_theta.norm(),d_theta.normalized());
state->G_q_I = old_state.G_q_I * d_rot.toRotationMatrix();
}
}
if(imu_data->quat.norm() != 0){
//current quaternion valid, haven't got last quaternion.
imu_data->last_quat = imu_data->quat;
}
//state->ab = old_state.ab;
//state->wb = old_state.wb;
}
2. Error-state 状态构建
然后就是根据当前状态进行获得含有累计误差的方程，来用于获取误差状态量更新和协方差矩阵的累计更新。 定义状态向量： 
于是误差状态方程可以写作：
其中: 
定义：
这是一个微分方程的闭合解，利用这个解进行递推会更精确。当然，也可以只取前面两项。
然后就可以完成误差姿态的更新，(这部分的累加看公式来说，并没有起到作用) 
协方差矩阵更新
Eigen::Matrix<double, 6, 1> SetCovarianceW(double gyro_noise, double accel_noise) {
I.setZero();
I.block<3,1>(0,0) = Eigen::Vector3d(accel_noise,accel_noise,accel_noise);
I.block<3,1>(3,0) = Eigen::Vector3d(gyro_noise,gyro_noise,gyro_noise);
}
inline Eigen::Matrix3d hat(const Eigen::Vector3d& v) {
Eigen::Matrix3d w;
w << 0., -v(2), v(1),
v(2), 0., -v(0),
-v(1), v(0), 0.;
return w;
}
void ImuProcessor::Imu_predict(const ImuDataPtr imu_data, double dt, State* state){
ESKF_update_nominal(imu_data,dt,state,gravity_);// 拿到state更新的结果
Eigen::Matrix<double, 6, 1> i = SetCovarianceW(0.01,0.01);
// 上文中的F_x
Eigen::Matrix<double,15,15> Fx = Eigen::Matrix<double,15,15>::Identity();
Fx.block<3,3>(0,3) = Eigen::Matrix3d::Identity() * dt;
Fx.block<3,3>(3,6) = - state->G_q_I*hat(imu_data->accel - state->ab)*dt;
Fx.block<3,3>(3,9) = - state->G_q_I*dt;
Eigen::Vector3d delta_angle_axis = (imu_data->gyro - state->wb) * dt;
if (delta_angle_axis.norm() >= 1e-12){
Fx.block<3,3>(6,6) = Eigen::AngleAxisd(delta_angle_axis.norm(),delta_angle_axis.normalized()).toRotationMatrix().transpose();
}else{
Fx.block<3,3>(6,6) = Eigen::Matrix3d::Identity();
}
Fx.block<3,3>(6,12) = -Eigen::Matrix3d::Identity()*dt;
// 上文中的F_i
Eigen::Matrix<double,15,12> Fi = Eigen::Matrix<double,15,12>::Zero();
Fi.block<12,12>(3,0) = Eigen::Matrix<double,12,12>::Identity();
// 上文中的Q
Eigen::Matrix<double,12,12> Q = Eigen::Matrix<double,12,12>::Zero();
Q.block<3,3>(0,0) = Eigen::Matrix3d::Identity() * am_noise_ * dt * dt;
Q.block<3,3>(3,3) = Eigen::Matrix3d::Identity() * wm_noise_ * dt * dt;
Q.block<3,3>(6,6) = Eigen::Matrix3d::Identity() * ab_noise_ * dt;
Q.block<3,3>(9,9) = Eigen::Matrix3d::Identity() * wb_noise_ * dt;
State old_state = *state;
state->del_x = Fx * old_state.del_x + Fi * i
state->P = (Fx * old_state.P * Fx.transpose() + Fi * Q * Fi.transpose()).eval();
}
3. Error State 测量更新
由于IMU的数据中夹带了大量噪声，我们需要利用更多的传感器的信息，来对状态进行修正。 而这些传感器中常用的组合包括：GPS+IMU，单目视觉+IMU，双目视觉+IMU等。
通常，量测方程的基本形式如下：
其中
是量测函数，与传感器有关。
是高斯白噪声，为了实现量测更新，我们需要求
对
的导数，所以有：
其中
是量测函数的雅可比矩阵，根据传感器的不同而具有不同形式。
卡尔曼增益的计算 
误差状态量测更新
协方差矩阵更新
Eigen::Vector3d z;
// gps vision
ConvertLLAToENU(state->lla_origin, GpsData->lla, &z);
Eigen::Vector3d h_x = state->G_p_I + state->G_q_I * I_p_Gps_;
Eigen::Matrix<double,3,15> H = Eigen::Matrix<double,3,15>::Zero();
H.block<3,3>(0,0) = Eigen::Matrix3d::Identity();
H.block<3,3>(0,6) = -state->G_q_I * hat(I_p_Gps_);
// gps noise
Eigen::Matrix3d V;
V << 1.5,0,0,0,1.5,0,0,0,4.0;
ESKF_correct(z,h_x,H,V,state);
void ESKF_correct(Eigen::VectorXd z,Eigen::VectorXd h_x, Eigen::MatrixXd H, Eigen::MatrixXd V, State* state){
State old_state = *state;
Eigen::MatrixXd K = old_state.P * H.transpose() * (H * old_state.P * H.transpose() + V).inverse();
state->del_x = K * (z - h_x);//这里是加入了GPS相减作为残差，但是如果没有gps可以通过IMU来赋给残差Eigen::VectorXd state->del_x = old_state.del_x，从而达到滤波的作用?
Eigen::MatrixXd I_KH = Eigen::Matrix<double, 15, 15>::Identity() - K * H;
state->P = (I_KH * old_state.P * I_KH.transpose() + K * V * K.transpose()).eval();
//inject error state
state->G_p_I = old_state.G_p_I + state->del_x.segment<3>(0);
state->G_v_I = old_state.G_v_I + state->del_x.segment<3>(3);
const Eigen::Vector3d del_theta = state->del_x.segment<3>(6);
if (del_theta.norm() >= 1e-12){
state->G_R_I = old_state.G_R_I * Eigen::AngleAxisd(del_theta.norm(),del_theta.normalized()).toRotationMatrix();
}
state->ab = old_state.ab + del_x.segment<3>(9);
state->wb = old_state.wb + del_x.segment<3>(12);
}
4. Error State 状态重置
由于nominal state的误差已经修正了，所以error state要置零，同时协方差矩阵也要相应的修改。 通常来说，重置的方法如下：
// 只将状态量置零
void ESKF::ResetState(State* state) {
state->del_x.setZero();
state->P= Eigen::Matrix<double,18,18>::Identity() * P * Eigen::Matrix<double,18,18>::Identity();
}
5. 参考链接
https://www.guyuehome.com/12615 https://blog.csdn.net/u011341856/article/details/114262451 https://github.com/je310/ESKF https://blog.csdn.net/liu3612162/article/details/114634670
评论（2）
您还未登录，请登录后发表或查看评论