>

## 确定误差标定

\begin{aligned} \boldsymbol{a}^O &= \boldsymbol{T}^a\boldsymbol{K}^a(\boldsymbol{a}^S + \boldsymbol{b}^a + \boldsymbol{n}^a) \\ \boldsymbol{\omega}^O &= \boldsymbol{T}^\omega\boldsymbol{K}^\omega(\boldsymbol{\omega} + \boldsymbol{b}^a + \boldsymbol{n}^a) \end{aligned}


\begin{aligned} \boldsymbol{a}^O &= \boldsymbol{T}^a\boldsymbol{K}^a\boldsymbol{a}^S + \boldsymbol{B}^a \\ \boldsymbol{\omega}^O &= \boldsymbol{T}^\omega\boldsymbol{K}^\omega\boldsymbol{\omega} + \boldsymbol{B}^g \end{aligned}


### 常规解方程方法

\begin{aligned} \begin{bmatrix} a_x^O\\ a_y^O\\ a_z^O \end{bmatrix} = \begin{bmatrix} 1 & -\beta_{yz}^a & \beta_{zy}^a \\ \beta_{xz}^a & 1 & -\beta_{zx}^a \\ -\beta_{xy}^a & \beta_{yx}^a & 1 \end{bmatrix} \begin{bmatrix} s_x^a & 0 & 0 \\ 0 & s_y^a & 0 \\ 0 & 0 & s_z^a \end{bmatrix} \begin{bmatrix} a_x^S\\ a_y^S\\ a_z^S \end{bmatrix} + \begin{bmatrix} B_x\\ B_y\\ B_z \end{bmatrix} \end{aligned}


#### 解析法（加速度计）

\begin{aligned} \boldsymbol{a}^S &= (\boldsymbol{T}^a\boldsymbol{K}^a)^{-1}\boldsymbol{a}^O - \boldsymbol{b}^a \\ &= \boldsymbol{T}'\boldsymbol{a}^O + \boldsymbol{b}'^{a}\\ &= \begin{bmatrix} s_x'^a & \beta'^a_{yz} & \beta'^a_{zy} \\ \beta'^a_{xz} & s_y'^a & \beta'^a_{zx} \\ \beta'^a_{xy} & \beta'^a_{yx} & s_z'^a \end{bmatrix}\boldsymbol{a}^O+ \begin{bmatrix} b'^a_x\\ b'^a_y\\ b'^a_z \end{bmatrix} \end{aligned}


\begin{aligned} a^S_x &= \beta'^a_{zy}g+b'^a_x \\ a^S_y &= \beta'^a_{zx}g+b'^a_x \\ a^S_z &= s_z'^ag+b'^a_x \end{aligned}


\begin{aligned} a'^S_x &= -\beta'^a_{zy}g+b'^a_x \\ a'^S_y &= -\beta'^a_{zx}g+b'^a_x \\ a'^S_z &= -s_z'^ag+b'^a_x \end{aligned}


#### 线性最小二乘（加速度计）

\begin{aligned} &\begin{aligned} a_x^O &= s_x^aa_x^S -\beta_{yz}^aa_y^S + \beta^a_{zy}a_z^S + b_x^a \\ a_y^O &= \beta_{xz}^aa_x^S -s_y^aa_y^S + -\beta_{zx}^aa_z^S + b_y^a \\ a_z^O &= -\beta_{xy}^aa_x^S -\beta_{yx}^aa_y^S + s_z^aa_z^S + b_z^a \end{aligned}\\ &\Downarrow \\ \begin{bmatrix} a_x^O\\ a_y^O\\ a_z^O \end{bmatrix} &= \begin{bmatrix} a_x^S & a_y^S & a_z^S & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & a_x^S & a_y^S & a_z^S & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & a_x^S & a_y^S & a_z^S & 1 \end{bmatrix} \begin{bmatrix} s_x^a\\ -\beta_{yz}^a\\ \beta^a_{zy}\\ b_x^a\\ \beta_{xz}^a\\ -s_y^a\\ -\beta_{zx}^a\\ b_y^a\\ -\beta_{xy}^a\\ -\beta_{yx}^a\\ s_z^a\\ b_z^a \end{bmatrix}\\ &\Downarrow\\ \boldsymbol{b} &= \boldsymbol{A}\boldsymbol{x} \end{aligned}


#### 解析法（陀螺仪）

• 不同于加速度计，陀螺仪没办法使用重力加速度作为真值，因此只能使用高精度转台来提供所需要的真值，而由于转台的角速度精度通常不如角度精度高，因此通常对角度进行积分得到角度作为真值
• 陀螺的角速度除了要考虑转台提供的角速度，还需要考虑地球自转的影响，通常的做法时通过构建特定的方程将地球转速抵消

\begin{aligned} \boldsymbol{\omega}^S &= (\boldsymbol{T}^g\boldsymbol{K}^g)^{-1}(\boldsymbol{\omega}^O + \boldsymbol{\omega}_e^i) - \boldsymbol{b}^g \\ &= \boldsymbol{T}'(\boldsymbol{\omega}^O + \boldsymbol{\omega}_e^i) + \boldsymbol{b}'^{g}\\ &= \begin{bmatrix} s_x'^g & \beta'^g_{yz} & \beta'^g_{zy} \\ \beta'^g_{xz} & s_y'^a & \beta'^g_{zx} \\ \beta'^g_{xy} & \beta'^g_{yx} & s_z'^g \end{bmatrix}(\boldsymbol{\omega}^O + \boldsymbol{\omega}_e^i)+ \begin{bmatrix} b'^g_x\\ b'^g_y\\ b'^g_z \end{bmatrix} \end{aligned}


\begin{aligned} \omega_x^S = \beta'^g_{zy}\omega + \begin{bmatrix} s_x'^g & \beta'^g_{yz} & \beta'^g_{zy} \end{bmatrix}\boldsymbol{\omega}_e^i + b'^g_x\\ \omega_x^S = \beta'^g_{zx}\omega + \begin{bmatrix} \beta'^g_{xz} & s_y'^a & \beta'^g_{zx} \end{bmatrix}\boldsymbol{\omega}_e^i + b'^g_y\\ \omega_x^S = s_z'^g\omega + \begin{bmatrix} \beta'^g_{xy} & \beta'^g_{yx} & s_z'^g \end{bmatrix}\boldsymbol{\omega}_e^i + b'^g_z \end{aligned}


\begin{aligned} \theta_x^S = \beta'^g_{zy}\theta + \begin{bmatrix} s_x'^g & \beta'^g_{yz} & \beta'^g_{zy} \end{bmatrix}\boldsymbol{\theta}_e^i + \theta'^g_{bx}\\ \theta_x^S = \beta'^g_{zx}\theta + \begin{bmatrix} \beta'^g_{xz} & s_y'^a & \beta'^g_{zx} \end{bmatrix}\boldsymbol{\theta}_e^i + \theta'^g_{by}\\ \theta_x^S = s_z'^g\theta + \begin{bmatrix} \beta'^g_{xy} & \beta'^g_{yx} & s_z'^g \end{bmatrix}\boldsymbol{\theta}_e^i + \theta'^g_{bz} \end{aligned}


\begin{aligned} \theta_x'^S = -\beta'^g_{zy}\theta + \begin{bmatrix} s_x'^g & \beta'^g_{yz} & \beta'^g_{zy} \end{bmatrix}\boldsymbol{\theta}_e^i + \theta'^g_{bx}\\ \theta_y'^S = -\beta'^g_{zx}\theta + \begin{bmatrix} \beta'^g_{xz} & s_y'^a & \beta'^g_{zx} \end{bmatrix}\boldsymbol{\theta}_e^i + \theta'^g_{by}\\ \theta_z'^S = -s_z'^g\theta + \begin{bmatrix} \beta'^g_{xy} & \beta'^g_{yx} & s_z'^g \end{bmatrix}\boldsymbol{\theta}_e^i + \theta'^g_{bz} \end{aligned}


$$\boldsymbol{\omega}^n = \begin{bmatrix} \omega^n_x \\ \omega^n_y \\ \omega^n_z \\ \end{bmatrix} = \begin{bmatrix} 0 \\ \omega_e\cos{\phi} \\ \omega_e\sin{\phi} \end{bmatrix}$$


$$\boldsymbol{\omega}^i_e = \boldsymbol{R}\begin{bmatrix} \omega^n_x \\ \omega^n_y \\ \omega^n_z \\ \end{bmatrix}$$


\begin{aligned} \boldsymbol{\omega}'^i_e &= \boldsymbol{R}_\pi\boldsymbol{R}\begin{bmatrix} \omega^n_x \\ \omega^n_y \\ \omega^n_z \\ \end{bmatrix}\\ &= \begin{bmatrix} \cos{\pi} & \sin{\pi} & 0\\ -\sin{\pi} & \cos{\pi} & 0\\ 0 & 0 & 1 \end{bmatrix}\boldsymbol{R}\begin{bmatrix} \omega^n_x \\ \omega^n_y \\ \omega^n_z \\ \end{bmatrix} \\ &= \begin{bmatrix} -1 & 0 & 0\\ 0 & -1 & 0\\ 0 & 0 & 1 \end{bmatrix}\boldsymbol{R}\begin{bmatrix} \omega^n_x \\ \omega^n_y \\ \omega^n_z \\ \end{bmatrix}\\ &= \boldsymbol{R}\begin{bmatrix} -\omega^n_x \\ -\omega^n_y \\ \omega^n_z \\ \end{bmatrix} \end{aligned}


\begin{aligned} \theta_x'^S = \begin{bmatrix} s_x'^g & \beta'^g_{yz} & \beta'^g_{zy} \end{bmatrix}\boldsymbol{\theta}_e^i + \theta'^g_{bx}\\ \theta_y'^S = \begin{bmatrix} \beta'^g_{xz} & s_y'^a & \beta'^g_{zx} \end{bmatrix}\boldsymbol{\theta}_e^i + \theta'^g_{by}\\ \theta_z'^S = \begin{bmatrix} \beta'^g_{xy} & \beta'^g_{yx} & s_z'^g \end{bmatrix}\boldsymbol{\theta}_e^i + \theta'^g_{bz} \end{aligned}


\begin{aligned} \theta_x^S &= s_x'^g\theta_{ex}^i + \theta'^g_{bx}\\ \theta_y^S &= s_y'^g\theta_{ey}^i + \theta'^g_{by}\\ \theta_z^S &= s_z'^g\theta_{ez}^i + \theta'^g_{bz} \end{aligned}


\begin{aligned} \theta_x'^S &= -s_x'^g\theta_{ex}^i + \theta'^g_{bx}\\ \theta_y'^S &= -s_y'^g\theta_{ey}^i + \theta'^g_{by}\\ \theta_z'^S &= s_z'^g\theta_{ez}^i + \theta'^g_{bz} \end{aligned}


### 优化方法

#### 优化方法标定加速度计内参

$$\boldsymbol{T}^a=\begin{bmatrix} 1 & -\beta_{yz}^a & \beta_{zy}^a \\ 0 & 1 & -\beta_{zx}^a \\ 0 & 0 & 1 \end{bmatrix}$$


$$\boldsymbol{\theta}^a = \begin{bmatrix} \beta_{yz}^a & \beta_{zy}^a & \beta_{zx}^a & s_x^a & s_y^a & s_z^a & b_x^a & b_y^a & b_z^a \end{bmatrix}$$


\begin{aligned} \boldsymbol{L}(\boldsymbol{\theta}^a) &= \sum_{k = 1}^M(||\boldsymbol{g}||^2 - ||h(\boldsymbol{a}_k^S, \boldsymbol{\theta}^a)||^2)^2 \\ h(\boldsymbol{a}_k^S, \boldsymbol{\theta}^a) &= \boldsymbol{T}^a\boldsymbol{K}^a(\boldsymbol{a}^S + \boldsymbol{b}^a) \end{aligned}


#### 优化方法标定陀螺仪内参

$$\boldsymbol{\theta}^a = \begin{bmatrix} \beta_{yz}^g & \beta_{zy}^g & \beta_{zx}^g & \beta^g_{xz} & \beta^g_{xy} & \beta^g_{yx} & s_x^g & s_y^g & s_z^g \end{bmatrix}$$


$$\boldsymbol{L}(\boldsymbol{\theta}^a) = \sum_{k = 1}^M(\boldsymbol{u}_{a, k} - \Psi(\omega_i^S, \boldsymbol{u}_{a, k - 1}))^2$$


## 随机误差标定

• (角度/速度)随机游走
• (角速度/加速度)随机游走
• 零偏不稳定性

Allan 方差标定的原理主要是利用功率谱的一些相关知识，由于我也不太熟悉所以这里不做深入，只总结了以下标定过程和怎么获取标定结果。过程如下：

• 将 IMU 静置一段时间，先按照周期 T 采集一段数据，对这组数据可以计算出其方差
• 将采集到的数据每两个进行平均，相当于我们 以 2T 周期采集了另一组数据，同样可以计算出相应的方差
• 用同样的方法获取不同周期下的方差数据，绘制方差-周期曲线(log-log 形式)

• 量化噪声(quantization noise)：斜率为 -1，转换关系：$y = \sqrt{3}Q$
• 白噪声/（角度/速度）随机游走(white noise/ (angle/velocity) random walk)： 斜率为 -1/2，转换关系：$y = N$
• 零偏不稳定性（bias instablity）：斜率为 0（曲线最低点），转换关系：$y = \frac{2B}{3}$
• （角速度/加速度）随机游走(angle velocity/acceralation random walk)：斜率为 1/2，转换关系：$y = \frac{K}{\sqrt{3}}$
• 速率斜坡（rate ramp）:斜率为 1，转换关系：$y = \frac{R}{\sqrt{2}}$

## 参考资料

Built with Hugo
Theme Stack designed by Jimmy