가속도계와 자력계 측정치를 사용한 자세 추정

가속도계와 자력계 측정치를 사용한 자세 추정¶

• Body frame에 장착된 accelerometer에서 측정된 측정치와 magnetometer 측정치를 사용하여 rotation matrx $R$을 구할 수 있다.
• Body frame에 장착된 각 센서는 3축으로 구성되며 측정전에 반드시 constant bias가 '0'이 되도록 calibration되어 있어야 한다.
• Calibration되어 있지 않은 센서에서 측정된 measurement vector를 사용하여 자세를 계산하게 되면, 자세오차가 매우 커지게 된다.
• 측정치를 사용하여 rotation matrix $R$은 다음과 같이 구할 수 있다.
• 이와 같은 방법은 TRIAD 방식과 같으며, 측정 vector를 사용한다고 해서 vector observation 방식의 attitude estimation이라 한다.

$$ \begin{aligned} R= \begin{bmatrix} (a \times m) \times a && a \times m && a \end{bmatrix} \end{aligned} $$

• 여기서 벡터 $a$는 unit vector이며, body frame의 accelerometer에서 측정된 가속도 측정치 $[a_x a_y z_z]$를 normalization 한 것 이며,

• 벡터 $m$은 unit vector이며, body frame의 magnetometer에서 측정된 자기장 측정치 $[m_x m_y m_z]$를 normalization 한 것이다.

• Reference:

https://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions

https://kr.mathworks.com/help/nav/ref/ecompass.html

In [1]:

import numpy as np
from scipy.io import loadmat
from navimath import *

In [9]:

# Example Data
ExData1 = loadmat('..\Data\logged_imu_data_Yes_sensor_bias.mat')

Accelerometer_NED = ExData1['Accelerometer_NED']
Accelerometer = ExData1['Accelerometer_IMU']

Gyroscope_NED = ExData1['Gyroscope_NED']
Gyroscope = ExData1['Gyroscope_IMU']

Magnetometer = ExData1['Magnetometer_IMU']

Orient_Quat_True = ExData1['Orientation_True_Quaternion']
Orient_Euler_True = ExData1['Orientation_True_Euler'] # yaw, pitch, roll
Orient_Quat_Est = ExData1['Orientation_Estimate_Quaternion'] # Orientation Quaternion Estimation

Ts = 1/ExData1['sample_per_sec'][0,0]
totalLen = Accelerometer.shape[0] 

In [3]:

# Direction cosine matrix to quaternion
# input: 
#  DCM: 3X3 Direction cosin matrix
# return:
#   quaternion a,b,c,d  or q0, q1, q2, q2 or qr, qi, qj, qk  
def DCM2Quaternion(DCM):
    quat = np.zeros((4))
    
    A11 = DCM[0,0]
    A12 = DCM[0,1]
    A13 = DCM[0,2]
    
    A21 = DCM[1,0]
    A22 = DCM[1,1]
    A23 = DCM[1,2]
    
    A31 = DCM[2,0]
    A32 = DCM[2,1]
    A33 = DCM[2,2]
    
    qr = np.sqrt(1.0/2.0)*(1+A11+A22+A33)
    qi = (1.0/(4.0*qr))*(A32-A23)
    qj = (1.0/(4.0*qr))*(A13-A31)
    qk = (1.0/(4.0*qr))*(A21-A12)
    
    return np.array([qr, qi, qj, qk])
    

In [4]:

# Attitude eatimation using Accelerometer and magnetometer measurement
# input:
#   Accelerometer: Acceleration measurement vector from body frame
#   Magnetometer: Magnetic measurement vector from body frame  
# return:
#   C: 3X3 direction cosine matrix
#   q: quaternion converted from C
def ecompass(Accelerometer, Magnetometer):
    a = Accelerometer/np.linalg.norm(Accelerometer)    # Accelerometer measurement normalization
    m = Magnetometer/np.linalg.norm(Magnetometer)      # Magnetometer measurement normalization
    
    aXm = np.cross(a,m)
    aXm = aXm/np.linalg.norm(aXm)
    aXmXa = np.cross(aXm,a)
    aXmXa = aXmXa / np.linalg.norm(aXmXa)
    
    C = np.array([np.transpose(aXmXa), np.transpose(aXm), np.transpose(a)])
    q = DCM2Quaternion(C)
    
    return C, q
    

In [5]:

Euler_hist = np.zeros((totalLen, 3))       # Euler estimated
Euler_true_hist = np.zeros((totalLen, 3))  # Euler true

for i in range(totalLen):
    C, q = ecompass(Accelerometer[i,:], Magnetometer[i,:])
    
    Euler_hist[i, :] = quaternion_to_euler(q)    
    Euler_true_hist[i,:] = quaternion_to_euler(Orient_Quat_True[i,:])    

In [6]:

plt.figure()

title=(['Roll angle','Pitch angle','Yaw angle'])

plt.figure(figsize=(20,20))
for i in range(3):
    plt.subplot(3,1,i+1)
    plt.plot(np.rad2deg(Euler_hist[:,i]))
    plt.plot(np.rad2deg(Euler_true_hist[:,i]))    
    plt.legend(['Estimated','True'])
    plt.title(title[i])
    plt.xlabel('Step')
    plt.ylabel('Degree')
    
    plt.grid()

<Figure size 432x288 with 0 Axes>

In [7]:

Euler_hist = np.zeros((int(totalLen*Ts), 3))
Euler_true_hist = np.zeros((int(totalLen*Ts), 3))

AcclerometerTmp = np.zeros((int(1/Ts),3))
MagmetometerTmp = np.zeros((int(1/Ts),3))

LoopCnt = 0
IndexCnt = 0

while (LoopCnt < totalLen):
    
    for i in range(int(1/Ts)):
        AcclerometerTmp[i,:] = Accelerometer[LoopCnt,:]
        MagmetometerTmp[i,:] = Magnetometer[LoopCnt,:]        
        LoopCnt = LoopCnt + 1
        
    aAve = np.mean(AcclerometerTmp, axis=0)
    mAve = np.mean(MagmetometerTmp, axis=0)   
        
    C, q = ecompass(aAve, mAve)
    
    Euler_hist[IndexCnt, :] = quaternion_to_euler(q)    
    Euler_true_hist[IndexCnt,:] = quaternion_to_euler(Orient_Quat_True[LoopCnt,:])    
    IndexCnt = IndexCnt + 1

Smoothing 데이터를 사용한 자세 추정¶

In [8]:

plt.figure()

title=(['Roll angle','Pitch angle','Yaw angle'])

plt.figure(figsize=(20,20))
for i in range(3):
    plt.subplot(3,1,i+1)
    plt.plot(np.rad2deg(Euler_hist[:,i]))
    plt.plot(np.rad2deg(Euler_true_hist[:,i]))    
    plt.legend(['Smmothed estimated','True'])
    plt.title(title[i])
    plt.xlabel('Step')
    plt.ylabel('Degree')
    
    plt.grid()

<Figure size 432x288 with 0 Axes>

ecompass.ipynb

546.6 kB

logged_imu_data_Yes_sensor_bias.mat

4.9 MB