2024年1月 – MathSword数值计算软件

1 背景
在使用传感器采集到一些数据后，需要计算车辆位姿。在融合GPS位置、IMU角速度、轮速后，可以通过ESKF进行求解。ESKF本质还是一个EKF过程，只是这里面更新量换成了值较小的误差状态量。由于是EKF过程，只需要导出下面公式(10)里的$F、F_i、H$矩阵，就可以使用EKF过程进行递推。

2 原理
设真值状态变量 $\mathbf{x}=[\mathbf{p},\theta,\mathbf{b}]$,其分别表示位置、朝向、车辆坐标系下角速度偏置，误差状态量为 $\delta\mathbf{x}=[\delta\mathbf{p},\delta\theta,\delta\mathbf{b}]$，$t$时刻含有噪声状态量为 $\mathbf{x}_t=[\mathbf{p}_t,\theta_t,\mathbf{b}_t]$。状态变量之间使用下式进行关联
(1)
$$
\begin{cases}
\mathbf{p}_t &=\mathbf{p}+\delta\mathbf{p}
\\
\theta_t&=\theta+\delta\theta&,R_t=\exp(\theta_t^\wedge) \approx \exp(\theta^\wedge)\exp(\delta\theta^\wedge)=R\exp(\delta\theta^\wedge)
\\
\mathbf{b}_t&=\mathbf{b}+\delta\mathbf{b}
\end{cases}
$$
以常见的四轮汽车模型为例，现在已知车辆坐标系下的后左右轮速$v_l,v_r$以及角速度$\bar{\mathbf{w}}$,这里以后左右轮中心作为车辆坐标系原点，设$\bar{\mathbf{v}}=[\dfrac{v_x+v_y}{2},0,0]$，速度与角速度噪声为$\eta_v,\eta_w$，接下来需要找到满足如下方程的$F,F_i$
(2)
$$
\delta\mathbf{x}=F\delta\mathbf{x}+F_i\begin{bmatrix}
\eta_v
\\
\eta_w
\end{bmatrix}
$$
根据物理模型有
(3)
$$
\begin{aligned}
&\begin{cases}
\dot{\mathbf{p}}_t &=R_t(\bar{\mathbf{v}}+\eta_v)
\\
\dot{R}_t&=R_t(\bar{\mathbf{w}}-\mathbf{b}_t-\eta_w)^\wedge
\\
\dot{\mathbf{b}}_t&=\eta_w
\end{cases}
\\
&\begin{cases}
\dot{\mathbf{p}} &=R\bar{\mathbf{v}}
\\
\dot{R}&=R(\bar{\mathbf{w}}-\mathbf{b})^\wedge
\\
\dot{\mathbf{b}}&=\mathbf{0}
\end{cases}
\end{aligned}
$$
对于位置更新，结合(1)(3)可得
(4)
$$
\begin{aligned}
\delta \dot{\mathbf{p}} &=\dot{\mathbf{p}}_t-\dot{\mathbf{p}}
\\
&=(R_t-R)\bar{\mathbf{v}}+R_t\eta_v
\\
&\approx (R\exp(\delta \theta^\wedge)-R)\bar{\mathbf{v}}+R\exp(\delta \theta^\wedge)\eta_v
\\
&\approx (R(I+\delta \theta^\wedge)-R)\bar{\mathbf{v}}+R(I+\delta \theta^\wedge)\eta_v
\\
&=R\delta\theta^\wedge\bar{\mathbf{v}}+R\delta\theta^\wedge\eta_v+R\eta_v
\\
&=-R(\bar{\mathbf{v}}+\eta_v)^\wedge\delta\theta+R\eta_v
\\
&\approx -R\bar{\mathbf{v}}^\wedge\delta\theta+R\eta_v
\end{aligned}
$$
对于旋转部分，结合(1)(3)可得
(5)
$$
\begin{aligned}
\dot{R}_t&=\dot{R}\exp(\delta\theta^\wedge)+R\dot{\exp(\delta \theta^\wedge)}
\\&=\dot{R}\exp(\delta\theta^\wedge)+R\exp(\delta\theta^\wedge)(\mathbf{J}_r(\delta\theta)\delta\dot{\theta})^\wedge
\\&\Downarrow
\\
\dot{R}_t&\approx\dot{R}\exp(\delta\theta^\wedge)+R\exp(\delta\theta^\wedge)\delta\dot{\theta}^\wedge
\\&\Downarrow
\\
R_t(\bar{\mathbf{w}}-\mathbf{b}_t-\eta_w)^\wedge&=R(\bar{\mathbf{w}}-\mathbf{b})^\wedge\exp(\delta\theta^\wedge)+R\exp(\delta\theta^\wedge)\delta\dot{\theta}^\wedge
\\&\Downarrow
\\
R\exp(\delta\theta^\wedge)(\bar{\mathbf{w}}-\mathbf{b}-\delta\mathbf{b}-\eta_w)^\wedge&=R(\bar{\mathbf{w}}-\mathbf{b})^\wedge\exp(\delta\theta^\wedge)+R\exp(\delta\theta^\wedge)\delta\dot{\theta}^\wedge
\\&\Downarrow
\\
\exp(\delta\theta^\wedge)(\bar{\mathbf{w}}-\mathbf{b}-\delta\mathbf{b}-\eta_w)^\wedge&=(\bar{\mathbf{w}}-\mathbf{b})^\wedge\exp(\delta\theta^\wedge)+\exp(\delta\theta^\wedge)\delta\dot{\theta}^\wedge
\\&\Downarrow
\\
\delta\dot{\theta}^\wedge&=(\bar{\mathbf{w}}-\mathbf{b}-\delta\mathbf{b}-\eta_w)^\wedge-\exp(-\delta\theta^\wedge)(\bar{\mathbf{w}}-\mathbf{b})^\wedge\exp(\delta\theta^\wedge)
\\
&\approx(\bar{\mathbf{w}}-\mathbf{b}-\delta\mathbf{b}-\eta_w)^\wedge-(I-\delta\theta^\wedge)(\bar{\mathbf{w}}-\mathbf{b})^\wedge(I+\delta\theta^\wedge)
\\
&=(\bar{\mathbf{w}}-\mathbf{b}-\delta\mathbf{b}-\eta_w)^\wedge-(\bar{\mathbf{w}}-\mathbf{b})^\wedge-(\bar{\mathbf{w}}-\mathbf{b})^\wedge\delta\theta^\wedge+\delta\theta^\wedge(\bar{\mathbf{w}}-\mathbf{b})^\wedge+\delta\theta^\wedge(\bar{\mathbf{w}}-\mathbf{b})^\wedge\delta\theta^\wedge
\\
&=-(\delta\mathbf{b}+\eta_w)^\wedge-(\bar{\mathbf{w}}-\mathbf{b})^\wedge\delta\theta^\wedge+\delta\theta^\wedge(\bar{\mathbf{w}}-\mathbf{b})^\wedge+\delta\theta^\wedge(\bar{\mathbf{w}}-\mathbf{b})
\\
&\approx -(\delta\mathbf{b}+\eta_w)^\wedge-(\bar{\mathbf{w}}-\mathbf{b})^\wedge\delta\theta^\wedge+\delta\theta^\wedge(\bar{\mathbf{w}}-\mathbf{b})^\wedge
\\
&= -(\delta\mathbf{b}+\eta_w)^\wedge+(\delta\theta^\wedge(\bar{\mathbf{w}}-\mathbf{b}))^\wedge
\\
&=-(\delta\mathbf{b}+\eta_w)^\wedge-((\bar{\mathbf{w}}-\mathbf{b})^\wedge\delta\theta)^\wedge
\\&\Downarrow
\\
\delta\dot{\theta}&=-(\delta\mathbf{b}+\eta_w)-(\bar{\mathbf{w}}-\mathbf{b})^\wedge\delta\theta
\end{aligned}
$$
对于角速度偏置部分，结合(1)(3)可以得到
(6)
$$
\delta\dot{\mathbf{b}}=\dot{\mathbf{b}}_t-\dot{\mathbf{b}} =\eta_w
$$
因此在一个小时间$\triangle t$内，综合(3)(4)(5)可以得到
(7)
$$
\begin{aligned}
&\begin{cases}
\delta \mathbf{p}_{t+\triangle t}&=\delta \mathbf{p}_t+\triangle t\delta \dot{\mathbf{p}}=\delta \mathbf{p}_t-\triangle tR\bar{\mathbf{v}}^\wedge \delta \theta_t+\triangle tR\eta_v
\\
\delta \theta_{t+\triangle t}&=\delta \theta_t+\triangle t\delta\dot{\theta}=\delta \theta_t-\triangle t(\bar{\mathbf{w}}-\mathbf{b})^\wedge\delta\theta_t-\triangle t(\delta\mathbf{b}_t+\eta_w)
\\
\delta\mathbf{b}_{t+\triangle t}&=\delta\mathbf{b}_t+\triangle t\delta\dot{\mathbf{b}}=\delta\mathbf{b}_t+\triangle t\eta_w
\end{cases}
\\
&\Downarrow
\\
&F=\begin{bmatrix}
I &-\triangle tR\bar{\mathbf{v}}^\wedge &\mathbf{0}
\\
\mathbf{0} & I-\triangle t(\bar{\mathbf{w}}-\mathbf{b})^\wedge &-\triangle tI
\\
\mathbf{0}&\mathbf{0} & I
\end{bmatrix}
\approx \begin{bmatrix}
I &-\triangle tR\bar{\mathbf{v}}^\wedge &\mathbf{0}
\\
\mathbf{0} & \exp(-\triangle t(\bar{\mathbf{w}}-\mathbf{b})^\wedge) &-\triangle tI
\\
\mathbf{0}&\mathbf{0} & I
\end{bmatrix}
\\
&F_i=\triangle t\begin{bmatrix}
R &\mathbf{0}
\\
\mathbf{0} &-I
\\
\mathbf{0} & I
\end{bmatrix}
\end{aligned}
$$
接下来，以GPS位置 $\bar{\mathbf{p}}$ 作为观测，因此建立观测函数
(8)
$$
\begin{aligned}
\bar{\mathbf{p}} &=z(\mathbf{x},\delta \mathbf{x})
\\
&=\mathbf{p}_t+\triangle tR_t\bar{\mathbf{v}}
\\
&=\mathbf{p}+\delta\mathbf{p}+\triangle tR\exp(\delta\theta^\wedge)\bar{\mathbf{v}}
\\
&\Downarrow
\\
&\begin{cases}
\dfrac{\partial\bar{\mathbf{p}}}{\partial\delta \mathbf{p}}&=I
\\
\\
\dfrac{\partial\bar{\mathbf{p}}}{\partial\delta \theta}&=\triangle tR\dfrac{\partial(\exp(\delta\theta^\wedge)\bar{\mathbf{v}})}{\partial\delta \theta}\approx-\triangle tR(\exp(\delta\theta^\wedge)\bar{\mathbf{v}})^\wedge
\\
\\
\dfrac{\partial\bar{\mathbf{p}}}{\partial\delta \mathbf{b}}&=\mathbf{0}
\end{cases}
\\
&\Downarrow
\\
H&=\begin{bmatrix}
I & -\triangle tR(\exp(\delta\theta^\wedge)\bar{\mathbf{v}})^\wedge &\mathbf{0}
\end{bmatrix}\approx
\begin{bmatrix}
I & -\triangle tR\bar{v}^\wedge&\mathbf{0}
\end{bmatrix}
\end{aligned}
$$
注意，$H$计算中，忽略了二阶小量，这里$\triangle t、\delta\theta$均为小量。

EKF更新完成后，需要对协方差矩阵$P$进行重置。设 $\delta \mathbf{x}'=[\delta \mathbf{p}’,\delta \theta’,\delta\mathbf{b}’]$为$\delta \mathbf{x}$受到均值 $\delta \bar{\mathbf{x}}=[\delta \bar{\mathbf{p}},\delta \bar{\theta},\delta\bar{\mathbf{b}}]$的影响,因此按照逆重构形式(减法)找到一个满足$\delta \mathbf{x}'=G\delta\mathbf{x}+…$形式的重置矩阵$G$:
(9)
$$
\begin{aligned}
&\begin{cases}
\delta \mathbf{p}'&=\delta\mathbf{p}-\delta\bar{\mathbf{p}}
\\
\\
\delta \theta'&=\log(\exp(\delta\theta'^\wedge))^\vee
\\
&\approx \log(\exp(\delta\theta^\wedge)\exp(-\delta\bar{\theta})^\wedge)^\vee
\\
&\approx (I-\dfrac{1}{2}\delta\bar{\theta}^\wedge)\delta\theta-\delta\bar{\theta}
\\
\\
\delta\mathbf{b}'&=\delta\mathbf{b}-\delta\bar{\mathbf{b}}
\end{cases}
\\
&\Downarrow
\\
&G=\begin{bmatrix}
I &\mathbf{0} & \mathbf{0}
\\
\mathbf{0} &I-\dfrac{1}{2}\delta\bar{\theta}^\wedge&\mathbf{0}
\\
\mathbf{0} & \mathbf{0} & I
\end{bmatrix}
\end{aligned}
$$

3 算法过程
因此，最终的ESKF过程为
(10)
$$
\begin{aligned}
\text{预测}:&\begin{cases}
\delta \mathbf{x}&=F\delta\mathbf{x}
\\
P&=FPF^T+F_iQF_i^T
\end{cases}
\\
\text{增益}:&\begin{cases}
K&=PH^T(HPH^T+V)^{-1}
\end{cases}
\\
\text{更新}:&\begin{cases}
\delta \mathbf{x}&=K(\bar{\mathbf{p}}-z(\mathbf{x},\delta\mathbf{x}))
\\
\mathbf{x}_{t+\triangle t}&=\mathbf{x}_t+\delta\mathbf{x}
\\
P&=(I-KH)P(I-KH)^T+KVK^T
\end{cases}
\\
\text{重置}:&\begin{cases}
P&=GPG^T
\end{cases}
\end{aligned}
$$
公式里$V$为$3\times 3$的GPS观察的协方差矩阵，而$Q$是噪声协方差矩阵，$Q=\mathbf{Diag}(Cov(\eta_v),Cov(\eta_w))$，特别的，对于如下形式的$Q$可进行简化
(11)
$$
\begin{aligned}
Q&=\begin{bmatrix}
\alpha I&\mathbf{0}
\\
\mathbf{0} &\beta I
\end{bmatrix}
\\
&\Downarrow
\\
F_iQF_i^T&=\triangle t^2\begin{bmatrix}
\alpha I &\mathbf{0} & \mathbf{0}
\\
\mathbf{0} &\beta I & \mathbf{0}
\\
\mathbf{0} & \mathbf{0} &\beta I
\end{bmatrix}
\end{aligned}
$$
注意:
① 公式(10)当中$P$的表达式与参考[2]里的不一致，公式(10)里的表达式不局限于$P$是否为方阵，且其有更好的数值稳定性。
② 由于 $\delta \bar{\theta}$为0附近一个小量，因此在大部分场景里，可以忽略不计，这时 $G$就等价于单位阵，因此不使用重置步骤。

③ $Q、V$由仪器提供，或者按经验设置。

④ 计算过程中，注意上面公式里的位置$\mathbf{p}$是全局坐标系，$\bar{\mathbf{v}}、\mathbf{w}、\mathbf{b}$为车辆局部坐标系，旋转矩阵$R$为局部坐标系到世界坐标系的表示方式。

参考
[1] Sola J. Quaternion kinematics for the error-state Kalman filter[J]. arXiv preprint arXiv:1711.02508, 2017.
[2] 高翔. 简明ESKF推导. 2022

1 背景
在一些算法设计中,可能需要算特定需求的向量。比如已知一个向量，现在想构建一个向量，使得构建向量与指定向量的夹角为指定角度。

2 原理
根据背景介绍，这里解决这么一类问题：已知向量 $\mathbf{a},\mathbf{b},\mathbf{c},\mathbf{d}$,以及参数 $\alpha,w_0,w_1$，且参数满足$|\mathbf{a}|\neq 0,|\mathbf{b}|\neq 0,|\mathbf{c}|\neq 0,|\mathbf{d}|\neq 0,0 < \alpha < 1,0<w_0<w_1$。现在求未知数$x,y,z,w$使得其结果满足如下约束
(1)
$$
\begin{cases}
\alpha &=\dfrac{(x\mathbf{a}+y\mathbf{b}+z\mathbf{c})\cdot \mathbf{d}}{|x\mathbf{a}+y\mathbf{b}+z\mathbf{c}||\mathbf{d}|}
\\
w&=|x\mathbf{a}+y\mathbf{b}+z\mathbf{c}|
\\
w_0&\leq w\leq w_1
\end{cases}
$$
为了求解(1)的问题，这里先记如下中间变量
(2)
$$
\begin{aligned}
v_1&=\alpha|\mathbf{d}|
\\
v_2&=\mathbf{a}\cdot\mathbf{d}
\\
v_3&=\mathbf{b}\cdot\mathbf{d}
\\
v_4&=\mathbf{c}\cdot\mathbf{d}
\\
v_5&=\alpha^2|\mathbf{d}|^2|\mathbf{a}|^2
\\
v_6&=\alpha^2|\mathbf{d}|^2|\mathbf{b}|^2
\\
v_7&=\alpha^2|\mathbf{d}|^2|\mathbf{c}|^2
\\
v_8&=\alpha^2|\mathbf{d}|^2\mathbf{a}\cdot\mathbf{b}
\\
v_9&=\alpha^2|\mathbf{d}|^2\mathbf{b}\cdot\mathbf{c}
\\
v_{10}&=\alpha^2|\mathbf{d}|^2\mathbf{c}\cdot\mathbf{a}
\\
v_{11}&=v_5-v_2^2
\\
v_{12}&=v_6-v_3^2
\\
v_{13}&=v_7-v_4^2
\\
v_{14}&=2(v_8-v_2v_3)
\\
v_{15}&=2(v_9-v_3v_4)
\\
v_{16}&=2(v_{10}-v_4v_2)
\\
v_{17}&=v_4v_{16}-v_2v_{13}
\\
v_{18}&=v_4v_{15}-v_3v_{13}
\\
v_{19}&=v_1v_{13}
\\
v_{20}&=v_{11}v_4^2-v_2v_{17}
\\
v_{21}&=v_{12}v_4^2-v_3v_{18}
\\
v_{22}&=v_{14}v_4^2-v_2v_{18}-v_3v_{17}
\\
v_{23}&=v_1v_{17}-v_2v_{19}
\\
v_{24}&=v_1v_{18}-v_3v_{19}
\\
v_{25}&=v_{22}^2-4v_{20}v_{21}
\\
v_{26}&=2v_{22}v_{23}-4v_{20}v_{24}
\\
v_{27}&=v_{23}^2-4v_{20}v_1v_{19}
\end{aligned}
$$

对(1)式的第1个表达式进行变形可以得到
(3)
$$
\begin{aligned}
&\dfrac{x\mathbf{a}\cdot\mathbf{d}+y\mathbf{b}\cdot\mathbf{d}+z\mathbf{c}\cdot\mathbf{d}}{\alpha|\mathbf{d}|}=|x\mathbf{a}+y\mathbf{b}+z\mathbf{c}|=w>0
\\
&\Downarrow
\\
&\dfrac{v_2x+v_3y+v_4z}{v_1}=w
\Rightarrow
z=\dfrac{v_1w-v_2x-v_3y}{v_4}
\end{aligned}
$$
对(3)式两边进行平方可以得到
(4)
$$
\begin{aligned}
&(\mathbf{a}\cdot\mathbf{d})^2x^2+(\mathbf{b}\cdot\mathbf{d})^2y^2+(\mathbf{c}\cdot\mathbf{d})^2z^2+2(\mathbf{a}\cdot\mathbf{d})(\mathbf{b}\cdot\mathbf{d})xy+2(\mathbf{b}\cdot\mathbf{d})(\mathbf{c}\cdot\mathbf{d})yz+2(\mathbf{c}\cdot\mathbf{d})(\mathbf{a}\cdot\mathbf{d})zx
\\
=&\alpha^2|\mathbf{d}|^2(|\mathbf{a}|^2x^2+|\mathbf{b}|^2y^2+|\mathbf{c}|^2z^2+2(\mathbf{a}\cdot\mathbf{b})xy+2(\mathbf{b}\cdot\mathbf{c})yz+2(\mathbf{c}\cdot\mathbf{a})zx)
\\
&\Downarrow
\\
&v_2^2x^2+v_3^2y^2+v_4^2z^2+2v_2v_3xy+2v_3v_4yz+2v_4v_2zx
\\
=&v_5x^2+v_6y^2+v_7z^2+2v_8xy+2v_9yz+2v_{10}zx
\\
&\Downarrow
\\
0=&(v_7-v_4^2)z^2+2((v_9-v_3v_4)y+(v_{10}-v_4v_2)x)z+(v_5-v_2^2)x^2+(v_6-v_3^2)y^2+2(v_8-v_2v_3)xy
\\
=&v_{13}z^2+(v_{15}y+v_{16}x)z+v_{11}x^2+v_{12}y^2+v_{14}xy
\\
=&\dfrac{v_1w-v_2x-v_3y}{v_4}(v_{13}\dfrac{v_1w-v_2x-v_3y}{v_4}+v_{15}y+v_{16}x)+v_{11}x^2+v_{12}y^2+v_{14}xy
\\
&\Downarrow
\\
0=&(v_1w-v_2x-v_3y)(v_{13}(v_1w-v_2x-v_3y)+v_4(v_{15}y+v_{16}x))+v_4^2(v_{11}x^2+v_{12}y^2+v_{14}xy)
\\
=&(v_1w-v_2x-v_3y)((v_4v_{16}-v_2v_{13})x+(v_4v_{15}-v_3v_{13})y+v_1v_{13}w)+v_4^2(v_{11}x^2+v_{12}y^2+v_{14}xy)
\\
=&(v_1w-v_2x-v_3y)(v_{17}x+v_{18}y+v_{19}w)+v_4^2(v_{11}x^2+v_{12}y^2+v_{14}xy)
\\
=&(v_{11}v_4^2-v_2v_{17})x^2+(v_{12}v_4^2-v_3v_{18})y^2+(v_{14}v_4^2-v_2v_{18}-v_3v_{17})xy+(v_1v_{17}-v_2v_{19})wx+(v_1v_{18}-v_3v_{19})wy+v_1v_{19}w
\\
=&v_{20}x^2+v_{21}y^2+v_{22}xy+v_{23}wx+v_{24}wy+v_1v_{19}w^2
\\
=&v_{20}x^2+(v_{22}y+v_{23}w)x+(v_{21}y^2+v_{24}wy+v_1v_{19}w^2)
\end{aligned}
$$
要对(4)进行求解,明显当$v_{20}=0$时，这时的解如下
(5)
$$
\begin{cases}
&\begin{cases}
0 \neq v_{22}y+v_{23}w
\\
x=-\dfrac{v_{21}y^2+v_{24}wy+v_1v_{19}w^2}{v_{22}y+v_{23}w}
\end{cases}&,\text{while } (v_{22}\neq 0 \text{ or } v_{23} \neq 0) \text { and } a_{20}=0
\\
&\begin{cases}
\mathbf{solve}:v_{21}y^2+v_{24}wy+v_1v_{19}w^2=0
\end{cases}&,\text{while } v_{22}= 0 \text{ and } v_{23} = 0 \text { and } a_{20}=0
\end{cases}
$$
这里考虑$v_{20}\neq 0$时，(4)的解必须满足如下求解条件
(6)
$$
\begin{aligned}
r&=(v_{22}y+v_{23}w)^2-4v_{20}(v_{21}y^2+v_{24}wy+v_1v_{19}w^2)
\\
&=v_{22}^2y^2+2v_{22}v_{23}wy+v_{23}^2w_2-4v_{20}(v_{21}y^2+v_{24}wy+v_1v_{19}w^2)
\\
&=(v_{22}^2y^2-4v_{20}v_{21})y^2+(2v_{22}v_{23}-4v_{20}v_{24})wy+(v_{23}^2-4v_{20}v_1v_{19})w^2
\\
&=v_{25}y^2+v_{26}wy+v_{27}w^2>0
\\
&\Downarrow
\\
r&=\begin{cases}
v_{26}wy+v_{27}w^2&,\text{while }v_{25}=0
\\
v_{25}(y+\dfrac{v_{26}}{v_{25}}w)^2+\dfrac{(4v_{25}v_{27}-v_{26}^2)}{4v_{25}}w^2&,\text{while }v_{25}\neq0
\end{cases}
\end{aligned}
$$
对于(6)，当$v_{25}= 0$时,可得如下 $y$ 解
(7)
$$
y:\begin{cases}
y>-\dfrac{v_{27}}{v_{26}}w &,\text{while }v_{26}\neq 0
\\
任意值 &,\text{while }v_{26}=0 \text{ and } v_{27} \geq 0
\\
无解,&,\text{while }v_{26}=0 \text{ and } v_{27} < 0
\end{cases}
$$
对于(6)，当$v_{25}\neq 0$时,可得如下 $y$ 解
(8)
$$
y:\begin{cases}
任意值&,\text{while }v_{25}>0 \text{ and } 4v_{25}v_{27} \geq v_{26}^2
\\
\\
y\geq \dfrac{\sqrt{v_{26}^2-4v_{25}v_{27}}-v_{26}}{2v_{25}}w&,\text{while }v_{25}>0 \text{ and } 4v_{25}v_{27} < v_{26}^2
\\
\\
y\leq -\dfrac{\sqrt{v_{26}^2-4v_{25}v_{27}}+v_{26}}{2v_{25}}w&,\text{while }v_{25}>0 \text{ and } 4v_{25}v_{27} < v_{26}^2
\\
\\
\dfrac{\sqrt{v_{26}^2-4v_{25}v_{27}}-v_{26}}{2v_{25}}w \leq y\leq -\dfrac{\sqrt{v_{26}^2-4v_{25}v_{27}}+v_{26}}{2v_{25}}w&,\text{while }v_{25}<0 \text{ and } 4v_{25}v_{27} \leq v_{26}^2
\\
\\
无解&,\text{while }v_{25}<0 \text{ and } 4v_{25}v_{27} > v_{26}^2
\end{cases}
$$
在(7)(8)中,$y$存在解时,其解可以表示为 $y=s_yw$形式，因此将求解结果代回(4)可以得到
(9)
$$
\begin{aligned}
0&=v_{20}x^2+(v_{22}y+v_{23}w)x+(v_{21}y^2+v_{24}wy+v_1v_{19}w^2)
\\
&=v_{20}x^2+(s_yv_{22}+v_{23})wx+(s_y^2v_{21}+s_yv_{24}+v_1v_{19})w^2
\\
&=v_{20}x^2-pwx+qw^2
\\
&\Downarrow
\\
x&=\begin{cases}
\dfrac{p+\sqrt{p^2-4v_{20}q}}{2v_{20}}w
\\
\\
\dfrac{p-\sqrt{p^2-4v_{20}q}}{2v_{20}}w
\end{cases},
\begin{cases}
p=-(s_yv_{22}+v_{23})
\\
q=s_y^2v_{21}+s_yv_{24}+v_1v_{19}
\end{cases}
\end{aligned}
$$
从(9)可以看出，$x$也可表示成 $x =s_xw$形式，最终将$x,y$代入(3)可以得到
(10)
$$
z=\dfrac{v_1-v_2s_x-v_3s_y}{v_4}w
$$
因此，在实际求解中，主要使用(5),(7),(8)获得可行解$y$，然后再计算$x,z$。

3 后记
在使用2的原理进行向量构造时，会存在如下问题：
① 由于输入向量特殊性，在求解中，有较大概率会触发无解情况。在自己使用数亿随机测试例子中, 会有将近30%的概率触发无解，好的一面是将近70%的数据能正常求解。
② 在使用(5)(6)(7)求解 $y$ 时，当 $y$在一个区间取值时，这时尽量选择区间中绝对值较小的 $y$值。这是因为在一些向量计算里，当$y$超过某些限值后，计算可能出现非常大的数，因为计算变量本身精度原因，可能造成计算结果存在差异，这点在公式(8)的第4个解里尤其要注意。
③ 从第2部分原理可以看出，有解时，最终的$x,y,z$均与$w$存在比例关系。因此在实际公式计算时，可以设$w=1$，当计算出$x,y,z$后，可再对$x,y,z$乘以一个满足(1)约束的$w$即可。

月度归档： 2024年1月

基于GPS、IMU、轮速的ESKF

计算指定角度向量