整体思路
状态估计的本质是最大后验估计，联合概率密度函数一步步转化到标量函数的和，最后线性化迭代求解。
\[\begin{align}
P(X|Z) &\propto P(X|Z)P(Z) \\
&= P(Z|X)P(X) \\
&= P(X) \prod P(z_{i}|x_{\{j\}})
\end{align}\]
利用负对数将乘积转化为加法，我们称之为 损失函数，每一个 $h$ 称之为残差函数，而残差函数的输入是流形，但输出一般已经在欧式空间上了，因此可以使用最原始的加减符号：
\[\begin{align}
- \log [P(X) \prod P(z_{i}|x_{\{j\}})]
&= \sum_{all\ h} h^T(x_{\{j\}}; z_{i}) h(x_{\{j\}}; z_{i}) \\
& \scriptsize{x_{j} 扩充成 x} \\
&\approx \sum_{all\ h} (h_0 + J^h_x \Delta x)^T(h_0 + J^h_x \Delta x) \\
& \scriptsize{把 h_0 + J^h_x \Delta x 堆叠成列向量，然后展开，符号不变} \\
& =
\begin{bmatrix}
\cdots & (h_0 + J^h_x\Delta x)^T & \cdots
\end{bmatrix}
\begin{bmatrix}
\vdots \\
h_0 + J^h_x \Delta x\\
\vdots
\end{bmatrix} \\
\end{align}\]
将上面的矩阵改写为更加紧凑的形式：
\[\begin{align}
\begin{bmatrix}
\vdots \\
h_0 + J^h_x \Delta x\\
\vdots
\end{bmatrix}
&=
\begin{bmatrix}
\vdots \\
h_0 \\
\vdots
\end{bmatrix}
+
\begin{bmatrix}
\vdots \\
J^h_x \\
\vdots
\end{bmatrix}
\Delta x \\
&= \mathbf{h} + \mathbf{J} \Delta x
\end{align}\]
利用紧凑的形式，继续展开损失函数：
\[\begin{align}
\sum_{all\ h} (h_0 + J^h_x \Delta x)^T(h_0 + J^h_x \Delta x)
& = (\mathbf{h} + \mathbf{J} \Delta x)^T(\mathbf{h} + \mathbf{J} \Delta x) \\
&= \mathbf{h}^T \mathbf{h} + 2\mathbf{h}^T \mathbf{J} \Delta x + \Delta x^T \mathbf{J}^T \mathbf{J} \Delta x \\
&= \alpha_0 + 2\mathbf{h}^T \mathbf{J} \Delta x + \Delta x^T \mathbf{J}^T \mathbf{J} \Delta x
\end{align}\]
其中，$\mathbf{h}$ 是很多 $h_0$ 的堆叠，$\Delta x$ 是所有状态变量的增量的堆叠，$\mathbf{J}$ 则是很多 $J^h_x$ 的堆叠，$J^h_x$ 是各个子损失函数对所有状态变量的雅可比矩阵。
展开后，可以看到有一个常数项，这对整个函数值的变化没有贡献，因此：
\[\min (2\mathbf{h}^T \mathbf{J} \Delta x + \Delta x^T \mathbf{J}^T \mathbf{J} \Delta x)
=
\min (\alpha_0 + 2\mathbf{h}^T \mathbf{J} \Delta x + \Delta x^T \mathbf{J}^T \mathbf{J} \Delta x)\]
令
\[g(\Delta x) = 2\mathbf{h}^T \mathbf{J} \Delta x + \Delta x^T \mathbf{J}^T \mathbf{J} \Delta x\]
求 g 对 $\Delta x$ 雅可比矩阵
\[\mathbf{J}^g_{\Delta x} = 2 \mathbf{h}^T\mathbf{J} + 2(\Delta x)^T\mathbf{J}^T\mathbf{J}\]
当其雅可比矩阵（其实是梯度的转置）为 零向量 时，函数取极小值：
\[\mathbf{J}^g_{\Delta x}= 0\]
\[2 \mathbf{h}^T\mathbf{J} + 2(\Delta x)^T\mathbf{J}^T\mathbf{J} = 0 \\\]
从而得到所谓的正规方程，求解该方程，便得到移动的方向和大小 $\Delta x$
\[\mathbf{J}^T\mathbf{J}\Delta x = -\mathbf{J}^T\mathbf{h}\]
最直接的求解方法为求逆：
\[\Delta x = -(\mathbf{J}^T\mathbf{J})^{-1}\mathbf{J}^T\mathbf{}h\]
但求逆的计算量大，因此通常会选择分解的方法来求解该正规方程。
李群函数的雅可比矩阵
从上一章可以看到，想要求解得到步长，需要获取得到一个大的、细长的矩阵 $J$，这个矩阵由多个矩阵块堆 $J^{h_k}_x$ 叠起来，每个子块表示某个测量约束对应的 $h_k$ 对所有状态变量的雅可比矩阵，因此当约束远大于状态变量个数的时候，J 就会看起来十分地细长。
从因子图的角度来看，IMU 预积分解决的问题是：约束的表达式与状态变量的值有关，也就是当状态变量更新后，
约束的表达式变化了，此时需要重新计算约束表达式里的一些参数，而不是能够重复利用固定的约束表达式。
而预计分则可以让原本会变的约束表达式变成固定的表达式，与状态变量无关。
预积分整体的推导思路
从两个最基本的方程组出发，一个是动力学方程组，另一个是最原始的测量方程组，之后利用匀速旋转和加速度的假设，进行普通的积分，然后将状态的增量都变换到相邻两帧中的第一帧机体坐标系下，再经过一系列的移项构造，使得方程右侧的参数可从上到下依次计算得出，且与状态无关，从而保证了状态在优化后发生值的变动，不会导致预计分方程右侧需要重新计算。
首先从最基本的动力学方程出发
\[\dot{R} = R [\omega] \\
\dot{v} = a \\
\dot{p} = v\]
其中，$R$ 是将一个向量从 局部（local） 坐标系下的表示变换到 全局（global） 坐标系下的表示。$\omega$ 是 local 坐标系下的机体运动角速度。
转换成积分形式
\[R_{i+1} = R_i \oplus \int_{t_i}^{t+1} \omega \mathbf{d}t \\
v_{i+1} = v_i + \int_{t_i}^{t+1} a \mathbf{d}t \\
p_{i+1} = p_i + \int_{t_i}^{t+1} v \mathrm{d}t\]
然后在这段时间内假设 $\omega$ 和 $a$ 是恒定值，也就是进行零阶近似
\[R_{i+1} = R_i \oplus \omega \Delta t \\
v_{i+1} = v_i + a \Delta t \\
p_{i+1} = p_i + v_i \Delta t + \frac12 a \Delta t^2\]
考虑 IMU 的最原始的测量模型：
\[a_m = R^T(a-g) + b_a + n_a \\
\omega_m = \omega + b_g + n_g\]
其中， $a_m$ 是局部坐标系下的加速度计测量值，$\omega_m$ 是。$a$ 是全局坐标系下的加速度真值，$g$ 是全局坐标系下的重力加速度，$b_a$ 是加速度计测量时的偏置，$n_a$ 是加速度计测量时的高斯白噪声。$\omega$ 是局部坐标系下的角速度真实值，$b_g$ 是陀螺仪的偏置，$n_g$ 是陀螺仪测量时的高斯白噪声。
把 原始测量模型 代入到 状态演进方程 上：
\[R_{i+1} = R_i \oplus (\omega_m - b_g - n_g) \Delta t \\
v_{i+1} = v_i + R_i (a_m - b_a - n_a) \Delta t + g \Delta t \\
p_{i+1} = p_i +v_i \Delta t + \frac12 R_i(a_m - b_a - n_a) \Delta t^2 + \frac12 g \Delta t^2\]
考虑 $i$ 时刻 和 $j$ 时刻之间的演进，并将这段时间分割成多个小时间块 $\Delta t_k, k = i, i+1, \cdots, j$，可以得到：
\[R_j = R_i \prod_{k = i}^{j-1} \mathrm{Exp}(\omega_{m_k} - b_g - n_g) \Delta t_k \\
v_j = v_i + \sum_{k=i}^{j-1} R_k(a_{m_k} - b_a - n_a) \Delta t_k + g \sum_{k=i}^{j-1} \Delta t_k \\
p_j = p_i + \sum_{k=i}^{j-1} v_k \Delta t_k + \frac12 \sum_{k=i}^{j-1} R_k(a_m - b_a -n_a) \Delta t\]
八架无人机，分为三组，需要在一个矩形拒止区域内搜索 0.5m*0.5m 大小的数字标识，数字对应于自身的组别。
策略
尽量少转弯，以节约电量、降低里程计崩溃概率。因此是沿着长边飞行。
有如下三种策略：
- 无论自己看到，还是别的队伍看到自己所属的地标，立刻飞过去降落
- 从知道自身地标位置的队伍视角看最贪心，里程计漂移最少
- 只有自己看到才立刻飞过去，别队看到则记录，待本列搜索区域搜索完成后才前往自身地标
- 都搜索完本列才去
第二种属于折中，具体考虑这种策略。假设三个数字标识均匀分布。
全覆盖，用时短，目前部署的贪心不保证全覆盖
三种响应
- 未到边界就降落
- 到边界已知目标位置
- 到边界未知位置
三个数字标识的相对位置可简化为九宫格，一共有999=729种可能的布局
都是情况1最好，如下表示：
- 111:333=27 特定三条带上
- 112 121 212: (2+3+4+3+4+5+4+5+6)*3=108 两条带上
- 122 212 221:(1+4+9 + (3+23+33)2 + 333)3=231一条带上 或 两条带上 或 三条带上
- 222:336 * 3 + 2(33*3)=216 两条的情况：1看两个、2看两个、3看两个；三条的情况，231和312没完全对上，两种
- 113 131 311: (4+3+2+3+2+1+2+1）*3=54
- 123 132 213 231 312 321:(2(1+3)+1(2+3))*6=78 2被1看或被3看，3没被看，3只能在1带上
- 223 232 322: 0 1被23看，2被13看，3没被看 倒退，全搜索，不可能发生3情况
- 333: 0
一架未知，可分三类，分别是一组二组三组
代码实现
无人机的“自主”行为由状态机来表示与实现。
Total views.
cowboys.
Hits