圆锥曲线轨道

轨道六根数指：
半长轴(a)、偏心率(e)，轨道倾角(i)，升交点赤经($\Omega$)、近地点幅角($\omega$)、真近点角($\phi$)。
其它参数：
角动量 $\vec{h}$、半通径 $p$、远拱点 $r_a$、周期 $T$。

$\begin{aligned} \mathcal{E} =& -\frac{\mu}{2a}=\frac{v^2}{2}-\frac{\mu}{r} = \frac{v_{inf}^2}{2} \\ \vec{h} =& \vec{r}\times\vec{v} \\ |\vec{h}| =& |\vec{r}||\vec{v}|\sin\theta \\ a =& \frac{p}{1-e^2}=-\frac{\mu}{2\mathcal{E}} = \frac{\mu r}{2\mu-rv^2} \\ p =& \frac{h^2}{\mu} \\ \vec{e} =& \frac{\vec{v}\times\vec{h}}{\mu} - \frac{\vec{r}}{|\vec{r}|} = (\frac{v^2}{\mu}-\frac{1}{r})\vec{r} - \frac{\vec{v}\cdot\vec{r}}{\mu}\vec{v} \\ e =& \sqrt{1-\frac{h^2}{a\mu}} \\ r =& \frac{p}{1+e\cos f} \\ f =& \arccos(\frac{1}{e}(\frac{h^2}{\mu r}-1)) \\ r_a =& a(1+e) \\ T =& 2\pi\sqrt{\frac{a^3}{\mu}} \end{aligned}$

由轨道元素计算位置和速度向量

$\begin{aligned} R =& \begin{bmatrix} c_\Omega c_\omega-s_\Omega c_i s_\omega & -c_\Omega s_\omega-s_\Omega c_i c_\omega & s_\Omega s_i \\ s_\Omega c_\omega+c_\Omega c_i s_\omega & -s_\Omega s_\omega+c_\Omega c_i c_\omega & -c_\Omega s_i \\ s_i s_\omega & s_i c_\omega & c_i \end{bmatrix} \\ \vec{r} =& R\begin{bmatrix} \displaystyle\frac{a(1-e^2)}{1+e\cos\theta}\cos\theta \\ \displaystyle\frac{a(1-e^2)}{1+e\cos\theta}\sin\theta \\ 0 \end{bmatrix} \\ \vec{v} =& \sqrt{\frac{\mu}{a(1-e^2)}}R\begin{bmatrix} -\sin\theta \\ e+\cos\theta \\ 0 \end{bmatrix} \end{aligned}$

平近点角(mean anomaly, $M$)
真近点角(true anomaly, $\theta$)
偏近点角(eccentric anomaly, $E$)

$\begin{aligned} & E-e\sin E=M \\ & \frac{\tan\frac{E}{2}}{\tan\frac{\theta}{2}} = \sqrt{\frac{1-e}{1+e}} \end{aligned}$

根据平近点角计算真近点角

$\begin{aligned} \theta =& M+\left(2e-{\frac {1}{4}}e^{3}\right)\sin {M} + {\frac {5}{4}}e^{2}\sin {2M} \\ &+ {\frac {13}{12}}e^{3}\sin {3M}+O(e^{4}) \end{aligned}$

已知 $\mu$、初始位置 $\vec{r}$ 和速度 $\vec{v}$ 计算经过指定时间 $T_0$ 后的位置和速度

根据初始位置速度计算轨道能量 $\mathcal{E}$ 和偏心率 $\vec{e}$
根据 $\mu$ 和 $\mathcal{E}$ 计算半长轴 $a$ 进而计算轨道周期 $T$
根据 $T_0$ 和 $T$ 计算平近点角进而计算偏近点角 $E$
根据 $\vec{e}$ 计算半短轴 $b$ 和焦距 $c$，由椭圆参数方程得到 $\vec{r}$
由 $\mathcal{E}$ 计算速度大小 $|\vec{v}|$
由偏心率公式计算速度 $\vec{v}$

$\begin{aligned} &\left[\begin{matrix} e \\ 0 \\ 0 \end{matrix}\right] = (\frac{\vec{v}\times\vec{h}}{\mu} - \frac{\vec{r}}{|\vec{r}|}) \left[\begin{matrix} r_x \\ r_y \\ 0 \end{matrix}\right] - \frac{v_xr_x+v_yr_y}{\mu} \left[\begin{matrix} v_x \\ v_y \\ 0 \end{matrix}\right] \\ & k = \frac{\vec{v}\times\vec{h}}{\mu} - \frac{\vec{r}}{|\vec{r}|} \\ & r_xv_x^2+r_yv_xv_y = \mu(kr_x-e) \\ & r_yv_y^2+r_xv_xv_y = \mu kr_y \\ & r_x^2v_x^2+r_xr_yv_xv_y = \mu(kr_x-e)r_x \\ & r_y^2v_y^2+r_xr_yv_xv_y = \mu kr_yr_y \\ & r_x^2v_x^2-r_y^2v_y^2 = \mu(kr_x-e)r_x - \mu kr_yr_y = k_1 \\ & v_x^2+v_y^2 = v^2 \\ & v_x^2 = \frac{r_y^2v^2 + k_1}{r^2} \\ & v_y^2 = \frac{r_x^2v^2 - k_1}{r^2} \\ &k_1 = \mu k(r_x^2-r_y^2) - er_x = (v^2 - \frac{\mu}{r})(r_x^2-r_y^2) - \mu er_x \end{aligned}$

四元数、欧拉角、旋转矩阵

四元数、欧拉角、旋转矩阵笔记

旋转坐标系下的速度和加速度

自用的刚体姿态动力学推导

QUEST 算法

QUEST（QUaternion ESTimator）已知多个参考向量 $y_k$ 和对应的多个观测向量 $x_k$，求旋转矩阵 $A$ 使得 $y=Rx$ 的误差最小，即最小化指标函数

$J_0=\frac{1}{2}\sum_{k=1}^n\alpha_k|y_k-Ax_k|^2$

或者最大化指标函数

$J=\sum_{k=1}^n\alpha_ky_k^\text{T}Ax_k$

其中 $y_k$ 和 $x_k$ 均为单位向量，$\alpha_k\ge 0$ 为加权因子，$\sum\alpha_k=1$，并且 $J=1-J_0$。

一组数据的情况

首先研究一个向量 $y=Ax$，设 $B=yx^\text{T}$，利用关于矩阵的迹的两个公式

$x^\text{T}y=\text{tr}(yx^\text{T})=\text{tr}(xy^\text{T}) \\ \text{tr}(AB)=\text{tr}(BA)$

则

$\begin{aligned} J=y^\text{T}Ax=\text{tr}(y(Ax)^\text{T})=\text{tr}(yx^\text{T}A^\text{T}) =\text{tr}(BA^\text{T})=\text{tr}(AB^\text{T}) \end{aligned}$

将四元数与旋转矩阵的关系 (四元数、欧拉角、旋转矩阵笔记)

$R=(w^2-q_v^\text{T}q_v)I+2q_vq_v^\text{T}-2wq_v^{\wedge}$

代入 $J$ 得

$\begin{aligned} J =& \text{tr}([(w^2-q_v^\text{T}q_v)I +2q_vq_v^\text{T}-2wq_v^{\wedge}]B^\text{T}) \\ =& (w^2-q_v^\text{T}q_v)\text{tr}(B)+2\text{tr}(q_vq_v^\text{T}B^\text{T}) -2w\text{tr}(q_v^{\wedge}B^\text{T}) \end{aligned}$

记

$\begin{aligned} \sigma =& \text{tr}(B) \\ S =& B+B^\text{T} = yx^\text{T}+xy^\text{T} \\ Z =& y\times x \end{aligned}$

则

$\begin{aligned} J=& (w^2-q_v^\text{T}q_v)\sigma+2\text{tr}(q_vq_v^\text{T}xy^\text{T}) -2w\text{tr}(q_v^{\wedge}xy^\text{T}) \\ =& w^2\sigma-q_v^\text{T}q_v\sigma+\text{tr}(xy^\text{T}q_vq_v^\text{T}) +q_v^\text{T}x\text{tr}(q_vy^\text{T}) -2w\text{tr}(q_v^{\wedge}xy^\text{T}) \\ =& w^2\sigma-q_v^\text{T}q_v\sigma+y^\text{T}q_v\text{tr}(xq_v^\text{T}) +q_v^\text{T}x\text{tr}(y^\text{T}q_v) -2wy^\text{T}q_v^{\wedge}x \\ =& w^2\sigma-\sigma q_v^\text{T}q_v+q_v^\text{T}yx^\text{T}q_v +q_v^\text{T}xy^\text{T}q_v+2wq_v^\text{T}y^{\wedge}x \\ =& w^2\sigma-\sigma q_v^\text{T}q_v+q_v^\text{T}Sq_v+2wq_v^\text{T}(y\times x) \\ =& w^2\sigma+q_v^\text{T}Zw+wZ^\text{T}q_v +q_v^\text{T}Sq_v-\sigma q_v^\text{T}q_v \\ =& (w\sigma+q_v^\text{T}Z)w+(wZ^\text{T}+q_v^\text{T}(S-\sigma I))q_v \\ =& \begin{bmatrix} w & q_v^\text{T} \end{bmatrix} \begin{bmatrix} \sigma & Z^\text{T} \\ Z & S-\sigma I \\ \end{bmatrix} \begin{bmatrix} w \\ q_v \end{bmatrix} \\ =& q^\text{T}Kq \end{aligned}$

其中第3行到第4行 $y^\text{T}q_v^{\wedge}x = -q_v^\text{T}y^{\wedge}x$ 实际上等价于

$a\cdot (b\times c) = b\cdot (c\times a) = c\cdot (a\times b)$

考虑到单位四元数 $q$ 满足 $|q|=1$，使用拉格朗日乘子法，

$\begin{aligned} L =& q^\text{T}Kq+\lambda(q^\text{T}q-1) \\ \frac{\partial L}{\partial q} =& 2Kq-2\lambda q=0 \end{aligned}$

进一步扩展到多组数据并加入权重，取

$\begin{aligned} B =& \sum_{k=1}^n \alpha_k y_k x_k^\text{T} \\ \sigma =& \text{tr}(B) \\ S =& B+B^\text{T} \\ Z =& \sum_{k=1}^n \alpha_k y_k \times x_k \end{aligned}$