多传感器融合定位（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)