多传感器融合定位（3-惯性导航原理及误差分析）7-实现 分立级标定 accel加速度计内参公式验证
多传感器融合定位（3-惯性导航原理及误差分析）7-实现 分立级标定 accel加速度计内参公式验证
参考博客：
组合导航系列文章（五）：IMU误差标定之基于转台的标定
基于Python的开源GNSS/INS仿真
github代码 hkwww/gnss-ins-sim
本次主要使用 解析法进行 accel 加速度计的内参公式验证
最终结果
搭建仿真场景
参考 gnss-ins-sim 中的 demo_allan.py 构建 imu 仿真模型，并保存数据
sim_imu.py 部分代码（全部代码在文末）
指定 场景设计motion的文件、fs 采样频率为100HZ (1秒读取100次)
# globals
D2R = math.pi/180
motion_def_path = os.path.abspath('.//demo_motion_def_files//')
fs = 100.0 # IMU sample frequency
imu_err = {'gyro_b': np.array([0.0, 0.0, 0.0]),
'gyro_arw': np.array([0.25, 0.25, 0.25]) * 1.0,
'gyro_b_stability': np.array([3.5, 3.5, 3.5]) * 1.0,
'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
'accel_b': np.array([0.0, 0.0, 0.0]),
'accel_vrw': np.array([0.03119, 0.03009, 0.04779]) * 1.0,
'accel_b_stability': np.array([4.29e-5, 5.72e-5, 8.02e-5]) * 1.0,
'accel_b_corr': np.array([200.0, 200.0, 200.0]),
'mag_std': np.array([0.2, 0.2, 0.2]) * 1.0
}
# do not generate GPS and magnetometer data
imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=False)
根据自定义的运动场景，进行数据仿真
#### start simulation
sim = ins_sim.Sim([fs, 0.0, 0.0],
motion_def_path+"//internal_calibration_imu.csv",
ref_frame=1,
imu=imu,
mode=None,
env=None,
algorithm=None)
sim.run(1)
# generate simulation results, summary, and save data to files
sim.results('') # save data files
自定义的运动场景 internal_calibration_imu.csv
根据解析法的公式，需要设置6个imu场景，分别是
IMU Z轴朝上，Z轴朝下，Y轴朝上，Y轴朝下，X轴朝上，X轴朝下
internal_calibration_imu.csv
每个场景持续时间 20s，采样频率100hz，一个场景生成 2000个个数据
生成的仿真数据 accel-0.csv
进行内参标定验证
internal_calibration.py 部分代码（全部代码在文末）
指定accel的仿真生成数据
imu_sim_path = 'demo_saved_data/2020-11-05-11-37-15/accel-0.csv'
数据处理，这里以Z轴朝向数据为例，在accel-0.csv表中，区间 1-2000代表Z朝上，区间 2000-4000代表 Z朝下，经多次试验得知，imu在仿真运动后的数据需要一段时间才能稳定，所以选择取每个场景中 2000个数据中的500个，再把500个数据取均值表征 IMU在这个状态下的每个轴加速度
truth_z_up = get_imu_mean_data(raw_data,1000,1500) # IMU Z轴朝上 ，取中间500个数据的均值
truth_z_down = get_imu_mean_data(raw_data, 3000, 3500) # IMU Z轴朝下
6个场景 IMU数据处理后结果
留意 g 重力加速度所在的位置
——————————————————Z轴——————————————————————
[[ 8.90824804e-05 -6.20375208e-05 -9.79496514e+00]]
[[ 1.48275457e-04 -1.10436760e-01 9.79419008e+00]]
——————————————————Y轴——————————————————————
[[ 3.06869928e-04 -9.79499159e+00 -4.09315067e-02]]
[[-1.73602602e-04 9.79438219e+00 6.92753399e-02]]
——————————————————x轴——————————————————————
[[ 9.79475112e+00 2.27139296e-04 -1.53404379e-02]]
[[-9.79437605e+00 2.96217751e-04 2.76735700e-02]]
自定义内参矩阵
#init internal_calibration
truth_bias = np.array([0.1,0.2,0.3]).reshape(3,-1)
calib_bias = np.zeros((3,1))
truth_sacle_factor = np.asarray([[0.0024,0 , 0],
[0 ,0.25, 0],
[0 ,0 ,0.26]])
cailb_sacle_factor = np.zeros((3, 3))
truth_installation_error = np.asarray([[0 ,-0.0333,0.064222],
[0.0343,0 ,0.00890 ],
[0.0080,0.00231,0 ]])
cailb_installation_error = np.zeros((3, 3))
internal_martix = truth_sacle_factor + truth_installation_error # internal_martix 内参误差矩阵，包含（标度因素，安装误差）
求解误差模型
#分别求解 对应的误差模型 error_matix_ (Ax Ay Az)
##z 朝上;朝下
error_matix_z_up = np.dot(internal_martix,truth_z_up.T) + truth_bias
error_matix_z_down = np.dot(internal_martix,truth_z_down.T) + truth_bias
##y 朝上;朝下
error_matix_y_up = np.dot(internal_martix,truth_y_up.T) + truth_bias
error_matix_y_down = np.dot(internal_martix,truth_y_down.T) + truth_bias
##x 朝上;朝下
error_matix_x_up = np.dot(internal_martix,truth_x_up.T) + truth_bias
error_matix_x_down = np.dot(internal_martix,truth_x_down.T) + truth_bias
反验证，求内参误差
#解析法求解 零偏误差
calib_bias = (error_matix_z_up + error_matix_z_down) / 2
#解析法求解 标度因子 安装误差矩阵
internal_martix_z = (error_matix_z_up - calib_bias) / -g
internal_martix_y = (error_matix_y_up - calib_bias) / -g
internal_martix_x = (error_matix_x_up - calib_bias) / g
最后结果
零偏误差
truth bias
[[0.1]
[0.2]
[0.3]]
calib_bias
[[0.1018152 ]
[0.18618827]
[0.29977257]]
标度因素
truth_sacle_factor
[[0.0024 0. 0. ]
[0. 0.25 0. ]
[0. 0. 0.26 ]]
cailb_sacle_factor
[[0.00211219 0. 0. ]
[0. 0.24849897 0. ]
[0. 0. 0.25984316]]
安装误差
truth_installation_error
[[ 0. -0.0333 0.064222]
[ 0.0343 0. 0.0089 ]
[ 0.008 0.00231 0. ]]
cailb_installation_error
[[ 0. -0.0328296 0.064374 ]
[ 0.03568285 0. 0.00748734]
[ 0.00761199 0.0033713 0. ]]
全部代码
sim_imu.py
import numpy as np
import os
import math
from gnss_ins_sim.sim import imu_model
from gnss_ins_sim.sim import ins_sim
# globals
D2R = math.pi/180
motion_def_path = os.path.abspath('.//demo_motion_def_files//')
fs = 100.0 # IMU sample frequency
def create_sim_imu(): #生成 imu 方针imu数据
### Customized IMU model parameters, typical for IMU381
imu_err = {'gyro_b': np.array([0.0, 0.0, 0.0]),
'gyro_arw': np.array([0.25, 0.25, 0.25]) * 1.0,
'gyro_b_stability': np.array([3.5, 3.5, 3.5]) * 1.0,
'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
'accel_b': np.array([0.0, 0.0, 0.0]),
'accel_vrw': np.array([0.03119, 0.03009, 0.04779]) * 1.0,
'accel_b_stability': np.array([4.29e-5, 5.72e-5, 8.02e-5]) * 1.0,
'accel_b_corr': np.array([200.0, 200.0, 200.0]),
'mag_std': np.array([0.2, 0.2, 0.2]) * 1.0
}
# do not generate GPS and magnetometer data
imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=False)
#### start simulation
sim = ins_sim.Sim([fs, 0.0, 0.0],
motion_def_path+"//internal_calibration_imu.csv",
ref_frame=1,
imu=imu,
mode=None,
env=None,
algorithm=None)
sim.run(1)
# generate simulation results, summary, and save data to files
sim.results('') # save data files
internal_calibration_imu.py
# -*- coding: utf-8 -*-
# Filename: demo_allan.py
"""
Test Sim with Allan analysis.
Created on 2018-01-23
@author: dongxiaoguang
"""
import os
import math
import numpy as np
from read_csv import getRawData
from sim_imu import create_sim_imu
from gnss_ins_sim.sim import imu_model
from gnss_ins_sim.sim import ins_sim
# globals
D2R = math.pi/180
imu_sim_path = 'demo_saved_data/2020-11-05-11-37-15/accel-0.csv'
fs = 100.0 # IMU sample frequency
g = 9.8 # gravity
def print_info(truth_bias,calib_bias,
truth_sacle_factor,cailb_sacle_factor,
truth_installation_error,cailb_installation_error):
print("truth bias")
print(truth_bias)
print("calib_bias")
print(calib_bias)
print("truth_sacle_factor")
print(truth_sacle_factor)
print("cailb_sacle_factor")
print(cailb_sacle_factor)
print("truth_installation_error")
print(truth_installation_error)
print("cailb_installation_error")
print(cailb_installation_error)
def get_imu_mean_data(raw_data,x1,x2): # 获取IMU data ，做均值处理，取中间的500次数据做平均; x1 x2 为输入的区间
truth_array = np.asarray(raw_data[x1:x2])
truth_array = truth_array.astype(np.float) # 将numpy 中的字符型数据 转换 为 浮点型
data_mean = np.mean(truth_array, axis=0) # IMU Z轴朝上 ，取中间500个数据的均值
data_mean = np.asarray([data_mean])
return data_mean
def accel_calibration():
#get imu sim data
raw_data = np.asarray([])
raw_data = getRawData(imu_sim_path) # 获取imu仿真的原始数据
#process imu data
##Z轴##
truth_z_up = get_imu_mean_data(raw_data,1000,1500) # IMU Z轴朝上 ，取中间500个数据的均值
truth_z_down = get_imu_mean_data(raw_data, 3000, 3500) # IMU Z轴朝下
# print("——————————————————Z轴——————————————————————")
# print(truth_z_up)
# print(truth_z_down)
##Y轴##
truth_y_up = get_imu_mean_data(raw_data,7000,7500) # IMU y轴朝上 ，取中间500个数据的均值
truth_y_down = get_imu_mean_data(raw_data, 9000, 9500) # IMU y轴朝下
# print("——————————————————Y轴——————————————————————")
# print(truth_y_up)
# print(truth_y_down)
##X轴##
truth_x_up = get_imu_mean_data(raw_data,13000,13500) # IMU x轴朝上 ，取中间500个数据的均值
truth_x_down = get_imu_mean_data(raw_data, 15000, 15500) # IMU x轴朝下
# print("——————————————————x轴——————————————————————")
# print(truth_x_up)
# print(truth_x_down)
#init internal_calibration
truth_bias = np.array([0.1,0.2,0.3]).reshape(3,-1)
calib_bias = np.zeros((3,1))
truth_sacle_factor = np.asarray([[0.0024,0 , 0],
[0 ,0.25, 0],
[0 ,0 ,0.26]])
cailb_sacle_factor = np.zeros((3, 3))
truth_installation_error = np.asarray([[0 ,-0.0333,0.064222],
[0.0343,0 ,0.00890 ],
[0.0080,0.00231,0 ]])
cailb_installation_error = np.zeros((3, 3))
internal_martix = truth_sacle_factor + truth_installation_error # internal_martix 内参误差矩阵，包含（标度因素，安装误差）
#分别求解 对应的误差模型 error_matix_ (Ax Ay Az)
##z 朝上;朝下
error_matix_z_up = np.dot(internal_martix,truth_z_up.T) + truth_bias
error_matix_z_down = np.dot(internal_martix,truth_z_down.T) + truth_bias
##y 朝上;朝下
error_matix_y_up = np.dot(internal_martix,truth_y_up.T) + truth_bias
error_matix_y_down = np.dot(internal_martix,truth_y_down.T) + truth_bias
##x 朝上;朝下
error_matix_x_up = np.dot(internal_martix,truth_x_up.T) + truth_bias
error_matix_x_down = np.dot(internal_martix,truth_x_down.T) + truth_bias
#解析法求解 零偏误差
calib_bias = (error_matix_z_up + error_matix_z_down) / 2
#解析法求解 标度因子 安装误差矩阵
internal_martix_z = (error_matix_z_up - calib_bias) / -g
internal_martix_y = (error_matix_y_up - calib_bias) / -g
internal_martix_x = (error_matix_x_up - calib_bias) / g
#给 cailb_sacle_factor 对角线赋值
cailb_sacle_factor[0,0] = internal_martix_x[0]
cailb_sacle_factor[1,1] = internal_martix_y[1]
cailb_sacle_factor[2,2] = internal_martix_z[2]
# 给 cailb_installation_error 非对角线元素赋值
cailb_installation_error[1,0] = internal_martix_x[1]
cailb_installation_error[2,0] = internal_martix_x[2]
cailb_installation_error[0,1] = internal_martix_y[0]
cailb_installation_error[2,1] = internal_martix_y[2]
cailb_installation_error[0,2] = internal_martix_z[0]
cailb_installation_error[1,2] = internal_martix_z[1]
print_info(truth_bias, calib_bias,truth_sacle_factor, cailb_sacle_factor,truth_installation_error, cailb_installation_error)
if __name__ == '__main__':
#create_sim_imu() #生成自定义的方针 imu 数据
accel_calibration() #进行imu 加速度计内参标定
read_csv.py
import csv
import numpy as np
def getRawData(path):
raw_data = []
csv_reader = csv.reader(open(path,'r'))
header = next(csv_reader) # read the head
for row in csv_reader:
raw_data.append(row)
raw_data = np.asarray(raw_data) # 转换为numpy格式
return raw_data
if __name__ == '__main__':
raw_data = []
csv_reader = csv.reader(open('demo_saved_data/2020-11-03-23-07-52/accel-0.csv','r'))
header = next(csv_reader) # read the head
for row in csv_reader:
raw_data.append(row)
raw_data = np.asarray(raw_data) # 转换为numpy格式
print(raw_data.shape)