目录
1. 对ROS: 生成静止 imu 数据,使用imu_utils标定IMU生成Allen方差曲线
- ros下编译
- 执行, 生成 imu.bag
- rosbag play -r 500 imgimu_utils.bag 回放
- 用imu_utils进行接收和分析
- 用imu_utils下的scripts/下的matlab 脚本画allan曲线
1.1 配置环境
安装ROS参考https://www.bilibili.com/video/BV1zt411G7Vn?p=5
使用基于ROS的imu_utils工具完成allan方差标定,环境配置和imu_utils使用参考博文:https://blog.csdn.net/cuifeng1993/article/details/107420874和https://blog.csdn.net/u011392872/article/details/95787486?utm_source=distribute.pc_relevant.none-task
其中我额外遇到的问题是要指定OpenCV版本(同时装了3和4该工具使用的是3)
参考https://blog.csdn.net/u011392872/article/details/95787486使用imu_utils工具
1.2 噪声分析
在Param.h
中设置的noise参数是连续时间下的,如下所示:
int imu_frequency = 200;
double imu_timestep = 1./imu_frequency;
// noise
double gyro_bias_sigma = 0.00005;
double acc_bias_sigma = 0.0005;
double gyro_noise_sigma = 0.015; // rad/s * 1/sqrt(hz)
double acc_noise_sigma = 0.019; // m/(s^2) * 1/sqrt(hz)
在imu.cpp
中加入噪声函数如下:
void IMU::addIMUnoise(MotionData& data)
{
std::random_device rd;
std::default_random_engine generator_(rd());
std::normal_distribution<double> noise(0.0, 1.0);
Eigen::Vector3d noise_gyro(noise(generator_),noise(generator_),noise(generator_));
Eigen::Matrix3d gyro_sqrt_cov = param_.gyro_noise_sigma * Eigen::Matrix3d::Identity();
data.imu_gyro = data.imu_gyro + gyro_sqrt_cov * noise_gyro / sqrt( param_.imu_timestep ) + gyro_bias_;
Eigen::Vector3d noise_acc(noise(generator_),noise(generator_),noise(generator_));
Eigen::Matrix3d acc_sqrt_cov = param_.acc_noise_sigma * Eigen::Matrix3d::Identity();
data.imu_acc = data.imu_acc + acc_sqrt_cov * noise_acc / sqrt( param_.imu_timestep ) + acc_bias_;
// gyro_bias update
Eigen::Vector3d noise_gyro_bias(noise(generator_),noise(generator_),noise(generator_));
gyro_bias_ += param_.gyro_bias_sigma * sqrt(param_.imu_timestep ) * noise_gyro_bias;
data.imu_gyro_bias = gyro_bias_;
// acc_bias update
Eigen::Vector3d noise_acc_bias(noise(generator_),noise(generator_),noise(generator_));
acc_bias_ += param_.acc_bias_sigma * sqrt(param_.imu_timestep ) * noise_acc_bias;
data.imu_acc_bias = acc_bias_;
}
可以看到该加噪声函数中对噪声进行了离散化处理
根据Technical Report: An introduction to inertial navigation Oliver J. Woodman Allen方差曲线分析结果如下图
理解IMU误差分析参考:https://blog.csdn.net/luoshi006/article/details/82657017 和 https://zhuanlan.zhihu.com/p/71202815
随机行走误差:相对于陀螺仪,这项误差的效果是积分得到的角度的误差是维纳过程(随机行走是维纳过程的时间离散化);相对于加速度计,这项误差是指积分得到的速度的误差是维纳过程
零偏不稳定性:一种非静态噪声,可以理解为高斯白噪声和维纳过程的混合。直观上理解,零偏不稳定性误差会使陀螺仪和加速度计的零偏随时间慢慢变化,逐渐偏离开机时校准的零偏误差;同时还会造成一部分随机行走误差的效果
(摘自:https://zhuanlan.zhihu.com/p/71202815)
•White Noise/Random Walk appears on the Allan variance plot as a slope with gradient –0.5. The random walk measurement for this noise (ARW for a rate-gyroscope, VRW for an accelerometer) is obtained by fitting a straight line through the slope and reading its value at 𝜏 = 1. ARW in gyroscope is a measure for gyro noise and is given in units of dps/rt(Hz).
•Bias Instability appears on the plot as a flat region around the minimum. The numerical val ue is the minimum value on the Allan deviation plot. For a gyroscope, the bias stability measures how the bias of the gyroscope changes over a specified period of time at constant temperature. This is typically presented in units of dps/sec or dps/hr
Note:
随机游走:斜率为 −0.5 ,与
τ
=
1
\tau =1
τ=1的交点;(gyro - ARW角度随机游走 , acc - VRW速度随机游走)
零偏不稳定度:斜率为 0 (水平)线段的最低点;
1.3 标定结果
生成data数据如下,但我未理解sim和非sim的数据画出的Allen曲线有什么区别(貌似是sim版本的是拟合过后用于画曲线的),回头有需要再仔细看看论文
使用imu_utils标定结果如下所示:
%YAML:1.0
---
type: IMU
name: mytest
Gyr:
unit: " rad/s"
avg-axis:
gyr_n: 2.1208761962850811e-01
gyr_w: 8.3045773665738672e-04
x-axis:
gyr_n: 2.0967935045690383e-01
gyr_w: 9.0081610946356823e-04
y-axis:
gyr_n: 2.1659648411957311e-01
gyr_w: 7.0126791192864034e-04
z-axis:
gyr_n: 2.0998702430904739e-01
gyr_w: 8.8928918857995149e-04
Acc:
unit: " m/s^2"
avg-axis:
acc_n: 2.7061754353710271e-01
acc_w: 3.3945936693285440e-03
x-axis:
acc_n: 2.7378794212494856e-01
acc_w: 3.3717227231656486e-03
y-axis:
acc_n: 2.6734085954843995e-01
acc_w: 3.3266392907721136e-03
z-axis:
acc_n: 2.7072382893791958e-01
acc_w: 3.4854189940478710e-03
由上图可知imu_utils工具标定的结果是Bias Instability不是课件中的Random Walk
噪声类型 | 实际大小 | 标定结果 | 标定结果连续化后 |
---|---|---|---|
陀螺仪高斯白噪声(gyr_n) | 0.015 | 2.1208761962850811e-01 | 0.01499685940450312 |
陀螺仪bias随机游走 | 0.00005 | ||
陀螺仪零偏不稳定度(gyr_w) | 8.3045773665738672e-04 | ||
加速度计高斯白噪声(acc_n) | 0.019 | 2.7061754353710271e-01 | 0.019135550014313107 |
加速度计bias随机游走 | 0.0005 | ||
加速度计零偏不稳定度(acc_w) | 3.3945936693285440e-03 |
高斯白噪声连续化方式:标定结果× Δ t \sqrt {\Delta t} Δt
对比高斯白噪声项设定的结果与标定结果基本一致
使用kalibr_allan工具可参考
https://blog.csdn.net/weixin_44456692/article/details/106818092
https://blog.csdn.net/baidu_41931307/article/details/102971466
matlab画出gyr结果:
matlab画出acc结果:
最后我是在windows下用matlab画出曲线
clear
close all
dt = dlmread('C:\my_file\slam\手写vio\第2章-IMU传感器\data\data_mytest_acc_t.txt');
data_x = dlmread('C:\my_file\slam\手写vio\第2章-IMU传感器\data\data_mytest_acc_x.txt');
data_y= dlmread('C:\my_file\slam\手写vio\第2章-IMU传感器\data\data_mytest_acc_y.txt');
data_z = dlmread('C:\my_file\slam\手写vio\第2章-IMU传感器\data\data_mytest_acc_z.txt');
data_draw=[data_x data_y data_z] ;
data_sim_x= dlmread('C:\my_file\slam\手写vio\第2章-IMU传感器\data\data_mytest_sim_acc_x.txt');
data_sim_y= dlmread('C:\my_file\slam\手写vio\第2章-IMU传感器\data\data_mytest_sim_acc_y.txt');
data_sim_z= dlmread('C:\my_file\slam\手写vio\第2章-IMU传感器\data\data_mytest_sim_acc_z.txt');
data_sim_draw=[data_sim_x data_sim_y data_sim_z] ;
figure
loglog(dt, data_draw , 'o');
%loglog(dt, data_sim_draw , '-');
xlabel('time:sec');
ylabel('Sigma:deg/h');
%legend('x','y','z');
grid on;
hold on;
loglog(dt, data_sim_draw , '-');
2. 对非 ros : 生成运动 imu 数据比较欧拉积分与中值积分
- 编译
- 执行bin/data_gen, 生成数据
- 执行python_tools/draw_trajectory.py 画出轨迹
- 换成中值积分, 再重做一遍上述1,2,3过程
MotionData imupose_last = imudata[i-1];
MotionData imupose = imudata[i];
//delta_q = [1 , 1/2 * thetax , 1/2 * theta_y, 1/2 * theta_z]
Eigen::Quaterniond dq;
Eigen::Vector3d dtheta_half = (imupose.imu_gyro + imupose_last.imu_gyro) * dt /4.0;
dq.w() = 1;
dq.x() = dtheta_half.x();
dq.y() = dtheta_half.y();
dq.z() = dtheta_half.z();
dq.normalize();
/// imu 动力学模型 欧拉积分
// Eigen::Vector3d acc_w = Qwb * (imupose.imu_acc) + gw; // aw = Rwb * ( acc_body - acc_bias ) + gw
// Qwb = Qwb * dq;
// Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
// Vw = Vw + acc_w * dt;
/// 中值积分
Eigen::Vector3d acc_w = (Qwb * dq * imupose.imu_acc + Qwb * imupose_last.imu_acc)/2.0 + gw; // aw = Rwb * ( acc_body - acc_bias ) + gw
Qwb = Qwb * dq;
Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
Vw = Vw + acc_w * dt;
欧拉积分结果:
中值积分结果:
由上述图像对比可知欧拉积分随着误差累积到最后与原曲线分离越来越大,而中值积分结果全程与原曲线贴合,明显好于欧拉积分
3. 论文阅读
k-1次B样条曲线的标准基函数表示为:
p
(
t
)
=
∑
i
=
0
n
p
i
B
i
,
k
(
t
)
\mathbf{p}(t)=\sum_{i=0}^{n} \mathbf{p}_{i} B_{i, k}(t)
p(t)=i=0∑npiBi,k(t)
其中
p
i
∈
R
N
\mathbf{p}_{i} \in \mathbb R^N
pi∈RN是
t
i
t_i
ti时刻的控制点,
B
i
,
k
(
t
)
B_{i,k}(t)
Bi,k(t)是基函数De Boor - Cox recursive formula由下图表示
累加形式写作:
p
(
t
)
=
p
0
B
~
0
,
k
(
t
)
+
∑
i
=
1
n
(
p
i
−
p
i
−
1
)
B
~
i
,
k
(
t
)
\mathbf{p}(t)=\mathbf{p}_{0} \tilde{B}_{0, k}(t)+\sum_{i=1}^{n}\left(\mathbf{p}_{i}-\mathbf{p}_{i-1}\right) \tilde{B}_{i, k}(t)
p(t)=p0B~0,k(t)+i=1∑n(pi−pi−1)B~i,k(t)
其中
B
~
i
,
k
(
t
)
=
∑
j
=
i
n
B
j
,
k
(
t
)
\tilde{B}_{i, k}(t)=\sum_{j=i}^{n}{B}_{j, k}(t)
B~i,k(t)=∑j=inBj,k(t)是累加基函数,通过控制点之间对数映射
Ω
i
=
log
(
T
w
,
i
−
1
−
1
T
w
,
i
)
∈
s
e
3
\Omega_{i}=\log \left(\mathbf{T}_{w, i-1}^{-1} \mathbf{T}_{w, i}\right) \in \mathfrak{s e} 3
Ωi=log(Tw,i−1−1Tw,i)∈se3代替差分,连乘的指数映射代替求和,重写上式描述
S
E
3
\mathbb {SE} 3
SE3中的轨迹,得到下式:
T
w
,
s
(
t
)
=
exp
(
B
~
0
,
k
(
t
)
log
(
T
w
,
0
)
)
∏
i
=
1
n
exp
(
B
~
i
,
k
(
t
)
Ω
i
)
\mathbf{T}_{w, s}(t)=\exp \left(\tilde{B}_{0, k}(t) \log \left(\mathbf{T}_{w, 0}\right)\right) \prod_{i=1}^{n} \exp \left(\tilde{B}_{i, k}(t) \Omega_{i}\right)
Tw,s(t)=exp(B~0,k(t)log(Tw,0))i=1∏nexp(B~i,k(t)Ωi)
其中
T
w
,
s
(
t
)
∈
S
E
3
\mathbf{T}_{w, s}(t) \in \mathbb{S} \mathbb{E} 3
Tw,s(t)∈SE3是样条上 t 时刻的位姿,
T
w
,
i
∈
S
E
3
\mathbf{T}_{w, i} \in \mathbb{S} \mathbb{E} 3
Tw,i∈SE3是控制点位姿
这篇文章研究的是三次B样条,即k=4。假设控制点在每段时间间隔内均匀分布。三次B样条中4个控制点影响 t 时刻样条曲线的值。即对于
t
∈
[
t
i
,
t
i
+
1
)
t\in [t_i,t_{i+1})
t∈[ti,ti+1)这段时间中一共有
[
t
i
−
1
,
t
i
,
t
i
+
1
,
t
i
+
2
]
[t_{i−1},t_i,t_{i+1},t_{i+2}]
[ti−1,ti,ti+1,ti+2]四个点来确定样条取值。利用
s
(
t
)
=
(
t
−
t
0
)
/
Δ
t
s(t)=\left(t-t_{0}\right) / \Delta t
s(t)=(t−t0)/Δt表示时间平均,在
t
i
t_i
ti时刻的控制点变换为由
s
i
∈
[
0
,
1
,
⋯
n
]
s_i \in [0,1, \cdots n]
si∈[0,1,⋯n]表示。给定
s
i
≤
s
(
t
)
<
s
i
+
1
s_{i} \leq s(t)<s_{i+1}
si≤s(t)<si+1,定义:
u
(
t
)
=
s
(
t
)
−
s
i
u(t)=s(t)-s_i
u(t)=s(t)−si。进一步得到:
B
~
(
u
)
=
C
[
1
u
u
2
u
3
]
,
B
˙
(
u
)
=
1
Δ
t
C
[
0
1
2
u
3
u
2
]
,
B
¨
(
u
)
=
1
Δ
t
2
C
[
0
0
2
6
u
]
,
C
=
1
6
[
6
0
0
0
5
3
−
3
1
1
3
3
−
2
0
0
0
1
]
\tilde{\mathbf{B}}(u)=\mathbf{C}\left[\begin{array}{c} 1 \\ u \\ u^{2} \\ u^{3} \end{array}\right], \quad \dot{\mathbf{B}}(u)=\frac{1}{\Delta t} \mathbf{C}\left[\begin{array}{c} 0 \\ 1 \\ 2 u \\ 3 u^{2} \end{array}\right], \quad \ddot{\mathbf{B}}(u)=\frac{1}{\Delta t^{2}} \mathbf{C}\left[\begin{array}{c} 0 \\ 0 \\ 2 \\ 6 u \end{array}\right], \quad \mathbf{C}=\frac{1}{6}\left[\begin{array}{cccc} 6 & 0 & 0 & 0 \\ 5 & 3 & -3 & 1 \\ 1 & 3 & 3 & -2 \\ 0 & 0 & 0 & 1 \end{array}\right]
B~(u)=C⎣⎢⎢⎡1uu2u3⎦⎥⎥⎤,B˙(u)=Δt1C⎣⎢⎢⎡012u3u2⎦⎥⎥⎤,B¨(u)=Δt21C⎣⎢⎢⎡0026u⎦⎥⎥⎤,C=61⎣⎢⎢⎡651003300−33001−21⎦⎥⎥⎤
则样条轨迹中位姿可以表示为:
T
w
,
s
(
u
)
=
T
w
,
i
−
1
∏
j
=
1
3
exp
(
B
~
(
u
)
j
Ω
i
+
j
)
\mathbf{T}_{w, s}(u)=\mathbf{T}_{w, i-1} \prod_{j=1}^{3} \exp \left(\tilde{\mathbf{B}}(u)_{j} \Omega_{i+j}\right)
Tw,s(u)=Tw,i−1j=1∏3exp(B~(u)jΩi+j)
则位姿对于时间的一阶二阶导数如下:
T
w
,
s
(
u
)
=
T
w
,
i
−
1
(
A
˙
0
A
1
A
2
+
A
0
A
˙
1
A
2
+
A
0
A
1
A
˙
2
)
T
¨
w
,
s
(
u
)
=
T
w
,
i
−
1
(
A
¨
0
A
1
A
2
+
A
0
A
¨
1
A
2
+
A
0
A
1
A
¨
2
+
2
(
A
0
A
˙
1
A
2
+
A
0
A
1
A
2
+
A
0
A
˙
1
A
˙
2
)
)
A
j
=
exp
(
Ω
i
+
j
B
~
(
u
)
j
)
,
A
j
=
A
j
Ω
i
+
j
B
˙
(
u
)
j
A
¨
j
=
A
˙
j
Ω
i
+
j
B
˙
(
u
)
j
+
A
j
Ω
i
+
j
B
¨
(
u
)
j
\begin{aligned} \mathbf{T}_{w, s}(u) &=\mathbf{T}_{w, i-1}\left(\dot{\mathbf{A}}_{0} \mathbf{A}_{1} \mathbf{A}_{2}+\mathbf{A}_{0} \dot{\mathbf{A}}_{1} \mathbf{A}_{2}+\mathbf{A}_{0} \mathbf{A}_{1} \dot{\mathbf{A}}_{2}\right) \\ \ddot{\mathbf{T}}_{w, s}(u) &=\mathbf{T}_{w, i-1}\left(\begin{array}{c} \ddot{\mathbf{A}}_{0} \mathbf{A}_{1} \mathbf{A}_{2}+\mathbf{A}_{0} \ddot{\mathbf{A}}_{1} \mathbf{A}_{2}+\mathbf{A}_{0} \mathbf{A}_{1} \ddot{\mathbf{A}}_{2}+ \\ 2\left(\mathbf{A}_{0} \dot{\mathbf{A}}_{1} \mathbf{A}_{2}+\mathbf{A}_{0} \mathbf{A}_{1} \mathbf{A}_{2}+\mathbf{A}_{0} \dot{\mathbf{A}}_{1} \dot{\mathbf{A}}_{2}\right) \end{array}\right) \\ \mathbf{A}_{j} &=\exp \left(\Omega_{i+j} \tilde{\mathbf{B}}(u)_{j}\right), \quad \mathbf{A}_{j}=\mathbf{A}_{j} \Omega_{i+j} \dot{\mathbf{B}}(u)_{j} \\ \ddot{\mathbf{A}}_{j} &=\dot{\mathbf{A}}_{j} \Omega_{i+j} \dot{\mathbf{B}}(u)_{j}+\mathbf{A}_{j} \Omega_{i+j} \ddot{\mathbf{B}}(u)_{j} \end{aligned}
Tw,s(u)T¨w,s(u)AjA¨j=Tw,i−1(A˙0A1A2+A0A˙1A2+A0A1A˙2)=Tw,i−1(A¨0A1A2+A0A¨1A2+A0A1A¨2+2(A0A˙1A2+A0A1A2+A0A˙1A˙2))=exp(Ωi+jB~(u)j),Aj=AjΩi+jB˙(u)j=A˙jΩi+jB˙(u)j+AjΩi+jB¨(u)j
使用沿着样条观察到的第一个位姿的逆深度
ρ
∈
R
+
\rho \in \mathbb{R}^{+}
ρ∈R+来参数化路标。通过
W
\mathcal{W}
W将在相机a,图像坐标为
p
a
∈
R
2
\mathbf{p}_{a}\in\mathbb R^2
pa∈R2处观察到的逆深度点,转变到在相机b中对应的图像坐标
p
b
∈
R
2
\mathbf{p}_{b}\in\mathbb R^2
pb∈R2:
p
b
=
W
(
p
a
;
T
b
,
a
,
ρ
)
=
π
(
[
K
b
∣
0
]
T
b
,
a
[
K
a
−
1
[
p
a
1
]
;
ρ
]
)
\mathbf{p}_{b}=\mathcal{W}\left(\mathbf{p}_{a} ; \mathbf{T}_{b, a}, \rho\right)=\pi\left(\left[\mathbf{K}_{b} \mid \mathbf{0}\right] \mathbf{T}_{b, a}\left[\mathbf{K}_{a}^{-1}\left[\begin{array}{c} \mathbf{p}_{a} \\ 1 \end{array}\right] ; \rho\right]\right)
pb=W(pa;Tb,a,ρ)=π([Kb∣0]Tb,a[Ka−1[pa1];ρ])
其中
π
(
P
)
=
1
P
2
[
P
0
,
P
1
]
⊤
\pi(\mathbf{P})=\frac{1}{\mathbf{P}_{2}}\left[\mathbf{P}_{0}, \mathbf{P}_{1}\right]^{\top}
π(P)=P21[P0,P1]⊤为投影变换,
K
a
,
K
b
∈
R
3
×
3
\mathbf{K}_{a},\mathbf{K}_{b} \in \mathbb R^{3\times 3}
Ka,Kb∈R3×3为内参矩阵
合成的加速度计和陀螺仪的测量值为:
Gyro
(
u
)
=
R
w
,
s
⊤
(
u
)
⋅
R
w
,
s
(
u
)
+
bias
Accel
(
u
)
=
R
w
,
s
⊤
(
u
)
⋅
(
s
¨
w
(
u
)
+
g
w
)
+
bias
\begin{aligned} \operatorname{Gyro}(u) &=\mathbf{R}_{w, s}^{\top}(u) \cdot \mathbf{R}_{w, s}(u)+\text { bias } \\ \operatorname{Accel}(u) &=\mathbf{R}_{w, s}^{\top}(u) \cdot\left(\ddot{\mathbf{s}}_{w}(u)+g_{w}\right)+\text { bias } \end{aligned}
Gyro(u)Accel(u)=Rw,s⊤(u)⋅Rw,s(u)+ bias =Rw,s⊤(u)⋅(s¨w(u)+gw)+ bias
其中角速度
R
˙
w
,
s
\dot \mathbf R_{w,s}
R˙w,s和加速度
s
¨
w
,
s
\ddot \mathbf s_{w,s}
s¨w,s分别是矩阵
T
˙
w
,
s
\dot \mathbf T_{w,s}
T˙w,s和
T
¨
w
,
s
\ddot \mathbf T_{w,s}
T¨w,s的子矩阵
通过最小化该代价函数来估计出待求参数:
E
(
θ
)
=
∑
p
^
m
(
p
^
m
−
W
(
p
r
;
T
c
,
s
T
w
,
s
(
u
m
)
−
1
T
w
,
s
(
u
r
)
T
s
,
c
,
ρ
)
)
Σ
p
2
+
∑
ω
^
m
(
ω
^
m
−
Gyro
(
u
m
)
)
Σ
ω
2
+
∑
a
^
m
(
a
^
m
−
Accel
(
u
m
)
)
Σ
a
2
\begin{aligned} E(\theta)=& \sum_{\hat{\mathbf{p}}_{m}}\left(\hat{\mathbf{p}}_{m}-\mathcal{W}\left(\mathbf{p}_{r} ; \mathbf{T}_{c, s} \mathbf{T}_{w, s}\left(u_{m}\right)^{-1} \mathbf{T}_{w, s}\left(u_{r}\right) \mathbf{T}_{s, c}, \rho\right)\right)_{\Sigma_{p}}^{2}+\\ & \sum_{\hat{\omega}_{m}}\left(\hat{\omega}_{m}-\operatorname{Gyro}\left(u_{m}\right)\right)_{\Sigma_{\omega}}^{2}+\sum_{\hat{\mathbf{a}}_{m}}\left(\hat{\mathbf{a}}_{m}-\operatorname{Accel}\left(u_{m}\right)\right)_{\Sigma_{\mathbf{a}}}^{2} \end{aligned}
E(θ)=p^m∑(p^m−W(pr;Tc,sTw,s(um)−1Tw,s(ur)Ts,c,ρ))Σp2+ω^m∑(ω^m−Gyro(um))Σω2+a^m∑(a^m−Accel(um))Σa2
附录:
运行结果记录:
jevin@jevin-GS65-Stealth-9SE:~/code/vio-homework/hw2/catkin_ws$ roslaunch imu_utils my.launch
... logging to /home/jevin/.ros/log/9013bd9e-cfe9-11ea-ad5d-803253b3bbcc/roslaunch-jevin-GS65-Stealth-9SE-18433.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
started roslaunch server http://jevin-GS65-Stealth-9SE:46443/
SUMMARY
========
PARAMETERS
* /imu_an/data_save_path: /home/jevin/code/...
* /imu_an/imu_name: mytest
* /imu_an/imu_topic: /imu
* /imu_an/max_cluster: 100
* /imu_an/max_time_min: 120
* /rosdistro: melodic
* /rosversion: 1.14.6
NODES
/
imu_an (imu_utils/imu_an)
ROS_MASTER_URI=http://localhost:11311
process[imu_an-1]: started with pid [18458]
[ INFO] [1595841813.568696727]: Loaded imu_topic: /imu
[ INFO] [1595841813.569384900]: Loaded imu_name: mytest
[ INFO] [1595841813.569616807]: Loaded data_save_path: /home/jevin/code/vio-homework/hw2/catkin_ws/src/imu_utils/data/
[ INFO] [1595841813.569835158]: Loaded max_time_min: 120
[ INFO] [1595841813.570047439]: Loaded max_cluster: 100
gyr x num of Cluster 100
gyr y num of Cluster 100
gyr z num of Cluster 100
acc x num of Cluster 100
acc y num of Cluster 100
acc z num of Cluster 100
wait for imu data.
gyr x numData 1385689
gyr x start_t 1.59584e+09
gyr x end_t 1.59585e+09
gyr x dt
-------------7203.93 s
-------------120.066 min
-------------2.00109 h
gyr x freq 192.352
gyr x period 0.00519881
gyr y numData 1385689
gyr y start_t 1.59584e+09
gyr y end_t 1.59585e+09
gyr y dt
-------------7203.93 s
-------------120.066 min
-------------2.00109 h
gyr y freq 192.352
gyr y period 0.00519881
gyr z numData 1385689
gyr z start_t 1.59584e+09
gyr z end_t 1.59585e+09
gyr z dt
-------------7203.93 s
-------------120.066 min
-------------2.00109 h
gyr z freq 192.352
gyr z period 0.00519881
Gyro X
C 0.590273 3145.15 -8.14929 -2.10651 0.218362
Bias Instability 0.000532552 rad/s
Bias Instability 0.000900816 rad/s, at 483.526 s
White Noise 754.554 rad/s
White Noise 0.209679 rad/s
bias -0.110683 degree/s
-------------------
Gyro y
C 0.821169 3141.9 28.0625 -0.836935 0.0544534
Bias Instability 0.000583942 rad/s
Bias Instability 0.000701268 rad/s, at 1074.14 s
White Noise 779.507 rad/s
White Noise 0.216596 rad/s
bias -0.198744 degree/s
-------------------
Gyro z
C -0.747342 3160.36 6.75637 -2.65341 0.187761
Bias Instability 1.89735e-06 rad/s
Bias Instability 0.000889289 rad/s, at 483.526 s
White Noise 755.857 rad/s
White Noise 0.209987 rad/s
bias -0.26921 degree/s
-------------------
==============================================
==============================================
acc x numData 1385689
acc x start_t 1.59584e+09
acc x end_t 1.59585e+09
acc x dt
-------------7203.93 s
-------------120.066 min
-------------2.00109 h
acc x freq 192.352
acc x period 0.00519881
acc y numData 1385689
acc y start_t 1.59584e+09
acc y end_t 1.59585e+09
acc y dt
-------------7203.93 s
-------------120.066 min
-------------2.00109 h
acc y freq 192.352
acc y period 0.00519881
acc z numData 1385689
acc z start_t 1.59584e+09
acc z end_t 1.59585e+09
acc z dt
-------------7203.93 s
-------------120.066 min
-------------2.00109 h
acc z freq 192.352
acc z period 0.00519881
acc X
C -3.41531e-05 0.0197389 -0.000712013 0.000278679 -9.85564e-07
Bias Instability 0.00337172 m/s^2
White Noise 0.273788 m/s^2
-------------------
acc y
C -2.27847e-05 0.0195971 -0.000509255 0.000183621 2.85732e-06
Bias Instability 0.00332664 m/s^2
White Noise 0.267341 m/s^2
-------------------
acc z
C -4.74226e-05 0.0199097 -0.00127044 0.000324339 4.08945e-06
Bias Instability 0.00348542 m/s^2
White Noise 0.270724 m/s^2
-------------------
[imu_an-1] process has finished cleanly
log file: /home/jevin/.ros/log/9013bd9e-cfe9-11ea-ad5d-803253b3bbcc/imu_an-1*.log
all processes on machine have died, roslaunch will exit
shutting down processing monitor...
... shutting down processing monitor complete
done