MSCKF第6讲：状态与协方差

2024-03-22 01:36
文章标签 状态 协方差 msckf

本文主要是介绍MSCKF第6讲：状态与协方差，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

MSCKF中状态与协方差

文章目录

  • MSCKF中状态与协方差
  • 1 状态
  • 2 IMU状态与噪声协方差
    • 2.1 相关定义
    • 2.2 运动方程—IMU连续噪声协方差
    • 2.3 误差状态协方差矩阵
  • 3 cam状态

1 状态

msckf_vio.h

状态 StateServer state_server;

注意到，实际上imu_state和cam_states近似同步，并不是所有的imu数据都会建立一个状态。但是会处理区间内所有的imu数据。

在这里插入图片描述

struct StateServer {IMUState imu_state;Cam cam_states;// State covariance matrix	Eigen::MatrixXd state_cov;				// 误差状态(包括IMU和Cam)协方差矩阵Eigen::Matrix<double, 12, 12> continuous_noise_cov;		// IMU连续噪声协方差 4*3，即运动方程中的噪声方差
};// state_cov = A*state_cov*A + continuous_noise_cov 就是满足高斯运算，误差状态协方差由上一个时刻递推，然后再加上噪声协方差矩阵。

  下面G表示世界系global，I表示IMU坐标系（作为body系），cam0表示左目相机系。关于坐标系表示的申明，下面这种表达方式 G I R ^I_G{R} GI​R等价于 R I G {R}_{IG} RIG​，是一种常见的转换关系。

2 IMU状态与噪声协方差

2.1 相关定义

(4+3+3+3+3)×1 = 16×1
X I = [ G I q ˉ T b g T G v I T b a T G p I T ] T \mathbf{X}_\mathrm{I}=\begin{bmatrix}^I_G\bar{q}^T&\mathbf{b}_g^T&^G\mathbf{v}_I^T&\mathbf{b}_a^T&^G\mathbf{p}_I^T\end{bmatrix}^T XI​=[GI​qˉ​T​bgT​​GvIT​​baT​​GpIT​​]T

  • G I q ˉ T ^I_G\bar{q}^T GI​qˉ​T表示 the rotation from frame {G} to frame {I}，描述世界系到惯性系的旋转，用单位四元数表示，4维。
  • G p I T ^G\mathbf{p}_I^T GpIT​和 G v I T ^G\mathbf{v}_I^T GvIT​表示位置和速度，即平移量，the IMU position and velocity with respect to {G},在世界系下表示。
  • b g T \mathbf{b}_g^T bgT​和 b a T \mathbf{b}_a^T baT​分别表示the biases affecting the gyroscope and accelerometer measurements，即陀螺仪和加速度计的偏差。

imu_state.h

状态量含义
Eigen::Vector4d orientation q i w q_{iw} qiw​
Eigen::Vector3d position t w i t_{wi} twi​表示IMU在世界坐标系中的位置
Eigen::Vector3d velocity w v ^wv wv表示IMU在世界坐标系中的速度
Eigen::Vector3d gyro_bias b g b_{g} bg​ 角速度偏差，IMU静态初始化计算
Eigen::Vector3d acc_bias b a b_{a} ba​加速度偏差，IMU静态初始化计算
Eigen::Matrix3d R_imu_cam0 R c 0 i R_{c_0i} Rc0​i​
Eigen::Vector3d t_cam0_imu t i c 0 t_{ic_0} tic0​​
static double gyro_noise、acc_noise n g n_{g} ng​与 n a n_{a} na​，均值为0的高斯噪声，主要利用协方差
static double gyro_bias_noise、acc_bias_noise n b g n_{bg} nbg​与 n b a n_{ba} nba​
static Eigen::Isometry3d T_imu_bodyidentity
static Eigen::Vector3d gravity w g ^wg wg
Eigen::Vector4d orientation_null可观性约束
Eigen::Vector3d position_null
Eigen::Vector3d velocity_null

  注意gyro_bias和gyro_noise的区别，一个是角速度偏置（均值），一个是角速度噪声方差

2.2 运动方程—IMU连续噪声协方差

   state_server.continuous_noise_cov是运动方程中的噪声方差，由使用者提供，计算一次后不再发生变化。

imu参数—噪声标准差


nh.param<double>("noise/gyro", IMUState::gyro_noise, 0.001);
nh.param<double>("noise/acc", IMUState::acc_noise, 0.01);
nh.param<double>("noise/gyro_bias", IMUState::gyro_bias_noise, 0.001);
nh.param<double>("noise/acc_bias", IMUState::acc_bias_noise, 0.01);
// 特征的噪声
nh.param<double>("noise/feature", Feature::observation_noise, 0.01);

对应方差

IMUState::gyro_noise *= IMUState::gyro_noise;
IMUState::acc_noise *= IMUState::acc_noise;
IMUState::gyro_bias_noise *= IMUState::gyro_bias_noise;
IMUState::acc_bias_noise *= IMUState::acc_bias_noise;
Feature::observation_noise *= Feature::observation_noise;

构建连续噪声协方差

state_server.continuous_noise_cov = Matrix<double, 12, 12>::Zero();
state_server.continuous_noise_cov.block<3, 3>(0, 0) = Matrix3d::Identity() * IMUState::gyro_noise;
state_server.continuous_noise_cov.block<3, 3>(3, 3) = Matrix3d::Identity() * IMUState::gyro_bias_noise;
state_server.continuous_noise_cov.block<3, 3>(6, 6) = Matrix3d::Identity() * IMUState::acc_noise;
state_server.continuous_noise_cov.block<3, 3>(9, 9) = Matrix3d::Identity() * IMUState::acc_bias_noise;

应用：卡尔曼滤波应用阶段–计算先验误差状态协方差矩阵

state_cov = A*state_cov*A + continuous_noise_cov

Matrix<double, 21, 21> Q =Phi * G * state_server.continuous_noise_cov * G.transpose() * Phi.transpose() * dtime;state_server.state_cov.block<21, 21>(0, 0) =Phi * state_server.state_cov.block<21, 21>(0, 0) * Phi.transpose() + Q;

2.3 误差状态协方差矩阵

配置文件yaml中提供了误差状态初始协方差state_server.state_cov

<!-- These values should be covariance --><param name="initial_covariance/velocity" value="0.25"/><param name="initial_covariance/gyro_bias" value="0.01"/><param name="initial_covariance/acc_bias" value="0.01"/><param name="initial_covariance/extrinsic_rotation_cov" value="2.742e-3"/><param name="initial_covariance/extrinsic_translation_cov" value="4e-4"/>

初始化：旋转和平移误差状态协方差初始置为0，每一个变量是3维矩阵，7个变量一共21维度。一开始这个误差状态协方差的值是通过配置文件给出的！

外参

(4+3)×1
C I q ⊤ I p C ⊤ _C^I\mathbf{q}^\top\quad{}^I\mathbf{p}_C^\top CI​q⊤IpC⊤​

state_server.state_cov = MatrixXd::Zero(21, 21);
for (int i = 3; i < 6; ++i)state_server.state_cov(i, i) = gyro_bias_cov;
for (int i = 6; i < 9; ++i)state_server.state_cov(i, i) = velocity_cov;
for (int i = 9; i < 12; ++i)state_server.state_cov(i, i) = acc_bias_cov;
for (int i = 15; i < 18; ++i)// 外参 Rbcstate_server.state_cov(i, i) = extrinsic_rotation_cov;
for (int i = 18; i < 21; ++i)// 外参tbcstate_server.state_cov(i, i) = extrinsic_translation_cov;

卡尔曼滤波预测阶段

state_server.state_cov.block<21, 21>(0, 0) =Phi * state_server.state_cov.block<21, 21>(0, 0) * Phi.transpose() + Q;

预测之后更新cam部分，就是imu变了，cam也改变

    // 7. 如果有相机状态量，那么更新imu状态量与相机状态量交叉的部分if (state_server.cam_states.size() > 0){// 起点是0 21  然后是21行 state_server.state_cov.cols() - 21 列的矩阵// 也就是整个协方差矩阵的右上角，这部分说白了就是imu状态量与相机状态量的协方差，imu更新了，这部分也需要更新state_server.state_cov.block(0, 21, 21, state_server.state_cov.cols() - 21) =Phi * state_server.state_cov.block(0, 21, 21, state_server.state_cov.cols() - 21);.....}

状态增广（new cam come from）后的误差状态协方差

// 4. 增广协方差矩阵
// 简单地说就是原来的协方差是 21 + 6n 维的，现在新来了一个伙计，维度要扩了
// 并且对应位置的值要根据雅可比跟这个时刻（也就是最新时刻）的imu协方差计算
// 4.1 扩展矩阵大小 conservativeResize函数不改变原矩阵对应位置的数值
// Resize the state covariance matrix.
size_t old_rows = state_server.state_cov.rows();
size_t old_cols = state_server.state_cov.cols();
state_server.state_cov.conservativeResize(old_rows + 6, old_cols + 6);

卡尔曼滤波更新阶段

// 4. 更新协方差
MatrixXd I_KH = MatrixXd::Identity(K.rows(), H_thin.cols()) - K * H_thin;
state_server.state_cov = I_KH * state_server.state_cov;

滑动窗口溢出后

if (cam_state_end < state_server.state_cov.rows())
{state_server.state_cov.block(cam_state_start, 0,state_server.state_cov.rows() - cam_state_end,state_server.state_cov.cols()) =state_server.state_cov.block(cam_state_end, 0,state_server.state_cov.rows() - cam_state_end,state_server.state_cov.cols());state_server.state_cov.block(0, cam_state_start,state_server.state_cov.rows(),state_server.state_cov.cols() - cam_state_end) =state_server.state_cov.block(0, cam_state_end,state_server.state_cov.rows(),state_server.state_cov.cols() - cam_state_end);state_server.state_cov.conservativeResize(state_server.state_cov.rows() - 6, state_server.state_cov.cols() - 6);
}
else
{state_server.state_cov.conservativeResize(state_server.state_cov.rows() - 6, state_server.state_cov.cols() - 6);
}

3 cam状态

  利用滑动窗口来记录的，不只是一帧的相机姿态。at any time instant the EKF state vector comprises (i) the evolving IMU state (ii)a history of up to N max past poses of the camera
X C = [ T m T ⋯ T n T ] T \mathbf{X}_\mathrm{C}=\begin{bmatrix}\mathbf{T}_m^T\cdots\mathbf{T}_n^T\end{bmatrix}^T XC​=[TmT​⋯TnT​​]T
  注意相机的每个T都是由 C q G ^C{q}_G CqG​和 G p C a m 0 ^G\mathbf{p}_{Cam0} GpCam0​表示（C也是cam0，简单表示，(4+3)×1×N ）

  说白了就是给每一帧创建了一个结构体，里面存储了基本变量，类似于ORB中的Frame

cam_state.h

状态量含义
Eigen::Vector4d orientation q c w q_{cw} qcw​
Eigen::Vector3d position t w c t_{wc} twc​
Eigen::Isometry3d T_cam0_cam1 T c 1 c 0 T_{c_1c_0} Tc1​c0​​
Eigen::Vector4d orientation_null修改测量雅可比矩阵，后面再细看
Eigen::Vector3d position_null
StateIDType id当前帧状态id
double time当前帧对应时间戳

这篇关于MSCKF第6讲：状态与协方差的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/834269。

相关文章

使用Python实现IP地址和端口状态检测与监控

使用Python实现IP地址和端口状态检测与监控

《使用Python实现IP地址和端口状态检测与监控》在网络运维和服务器管理中,IP地址和端口的可用性监控是保障业务连续性的基础需求,本文将带你用Python从零打造一个高可用IP监控系统,感兴趣的小伙... 目录概述：为什么需要IP监控系统使用步骤说明1. 环境准备2. 系统部署3. 核心功能配置系统效果展
阅读更多...
SpringSecurity JWT基于令牌的无状态认证实现

SpringSecurity JWT基于令牌的无状态认证实现

《SpringSecurityJWT基于令牌的无状态认证实现》SpringSecurity中实现基于JWT的无状态认证是一种常见的做法,本文就来介绍一下SpringSecurityJWT基于令牌的无... 目录引言一、JWT基本原理与结构二、Spring Security JWT依赖配置三、JWT令牌生成与
阅读更多...
关于WebSocket协议状态码解析

关于WebSocket协议状态码解析

《关于WebSocket协议状态码解析》：本文主要介绍关于WebSocket协议状态码的使用方式,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录WebSocket协议状态码解析1. 引言2. WebSocket协议状态码概述3. WebSocket协议状态码详解3
阅读更多...
Flutter监听当前页面可见与隐藏状态的代码详解

Flutter监听当前页面可见与隐藏状态的代码详解

《Flutter监听当前页面可见与隐藏状态的代码详解》文章介绍了如何在Flutter中使用路由观察者来监听应用进入前台或后台状态以及页面的显示和隐藏,并通过代码示例讲解的非常详细,需要的朋友可以参考下... flutter 可以监听 app 进入前台还是后台状态，也可以监听当http://www.cppcn
阅读更多...
MySQL 中的服务器配置和状态详解(MySQL Server Configuration and Status)

MySQL 中的服务器配置和状态详解(MySQL Server Configuration and Status)

《MySQL中的服务器配置和状态详解(MySQLServerConfigurationandStatus)》MySQL服务器配置和状态设置包括服务器选项、系统变量和状态变量三个方面,可以通过... 目录mysql 之服务器配置和状态1 MySQL 架构和性能优化1.1 服务器配置和状态1.1.1 服务器选项
阅读更多...
linux进程D状态的解决思路分享

linux进程D状态的解决思路分享

《linux进程D状态的解决思路分享》在Linux系统中,进程在内核模式下等待I/O完成时会进入不间断睡眠状态（D状态）,这种状态下,进程无法通过普通方式被杀死,本文通过实验模拟了这种状态,并分析了如... 目录1. 问题描述2. 问题分析3. 实验模拟3.1 使用losetup创建一个卷作为pv的磁盘3.
阅读更多...
Java实现状态模式的示例代码

Java实现状态模式的示例代码

《Java实现状态模式的示例代码》状态模式是一种行为型设计模式,允许对象根据其内部状态改变行为,本文主要介绍了Java实现状态模式的示例代码,文中通过示例代码介绍的非常详细,需要的朋友们下面随着小编来... 目录一、简介1、定义2、状态模式的结构二、Java实现案例1、电灯开关状态案例2、番茄工作法状态案例
阅读更多...
通过prometheus监控Tomcat运行状态的操作流程

通过prometheus监控Tomcat运行状态的操作流程

《通过prometheus监控Tomcat运行状态的操作流程》文章介绍了如何安装和配置Tomcat,并使用Prometheus和TomcatExporter来监控Tomcat的运行状态,文章详细讲解了... 目录Tomcat安装配置以及prometheus监控Tomcat一. 安装并配置tomcat1、安装
阅读更多...
Linux之进程状态&&进程优先级详解

Linux之进程状态&&进程优先级详解

《Linux之进程状态&&进程优先级详解》文章介绍了操作系统中进程的状态,包括运行状态、阻塞状态和挂起状态,并详细解释了Linux下进程的具体状态及其管理,此外,文章还讨论了进程的优先级、查看和修改进... 目录一、操作系统的进程状态1.1运行状态1.2阻塞状态1.3挂起二、linux下具体的状态三、进程的
阅读更多...
hdu1043（八数码问题，广搜 + hash（实现状态压缩） ）

hdu1043（八数码问题，广搜 + hash（实现状态压缩） ）

利用康拓展开将一个排列映射成一个自然数，然后就变成了普通的广搜题。 #include<iostream>#include<algorithm>#include<string>#include<stack>#include<queue>#include<map>#include<stdio.h>#include<stdlib.h>#include<ctype.h>#inclu
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2