流型上的Kalman Filter

同构(isomorphism)是代数中的概念，在双射的基础上保持代数运算。
同胚(homeomorphism)是拓扑中的概念，在双射的基础上，保持开集的对应（即映射和逆映射都是连续的）。
同构说明代数结构相同，同胚说明拓扑结构相同。当然，在Algebraic Topology 中，研究拓扑群等对象时，两者又会联系在一起。

连续时间系统运动学

The true-state kinematics

$$\begin{array}{rl}\dot{\mathbf{p}}& =\mathbf{v}\\ \dot{\mathbf{v}}& =\mathbf{a}\\ \dot{\mathbf{q}}& =\frac{1}{2}\mathbf{q}\otimes \mathbf{\omega }\\ {\dot{\mathbf{b}}}_a& ={\mathbf{n}}_a\\ {\dot{\mathbf{b}}}_{\mathbf{\omega }}& ={\mathbf{n}}_{\mathbf{\omega }}\\ \dot{\mathbf{g}}& =0\end{array}$$
真值状态的观测来源于IMU测量的加速度和角速度(IMU系)
$$\begin{array}{rl}& \{\begin{array}{l}{\mathbf{a}}_m={\mathbf{R}}^T\left(\mathbf{a}-\mathbf{g}\right)+{\mathbf{b}}_a+{\mathbf{n}}_a\\ {\mathbf{\omega }}_m=\mathbf{\omega }+{\mathbf{b}}_{\mathbf{w}}+{\mathbf{n}}_{\mathbf{\omega }}\end{array}\\ ⟹& \{\begin{array}{l}\mathbf{a}=\mathbf{R}\left({\mathbf{a}}_m-{\mathbf{b}}_a-{\mathbf{n}}_a\right)+\mathbf{g}\\ \mathbf{\omega }={\mathbf{\omega }}_m-{\mathbf{b}}_{\mathbf{\omega }}-{\mathbf{n}}_{\mathbf{\omega }}\end{array}\end{array}$$
代入可得

$$\begin{array}{rl}\dot{\mathbf{p}}& =\mathbf{v}\\ \dot{\mathbf{v}}& =\mathbf{R}\left({\mathbf{a}}_m-{\mathbf{b}}_a-{\mathbf{n}}_a\right)+\mathbf{g}\\ \dot{\mathbf{q}}& =\frac{1}{2}\mathbf{q}\otimes \left({\mathbf{\omega }}_m-{\mathbf{b}}_{\mathbf{\omega }}-{\mathbf{n}}_{\mathbf{\omega }}\right)\\ {\dot{\mathbf{b}}}_a& ={\mathbf{n}}_{\mathbf{ba}}\\ {\dot{\mathbf{b}}}_{\mathbf{\omega }}& ={\mathbf{n}}_{\mathbf{b}\mathbf{\omega }}\\ \dot{\mathbf{g}}& =0\end{array}\text{\hspace{1em}(true state)}$$
写成更简便的形式
$$\dot{\mathbf{x}}=f(\mathbf{x},\mathbf{u},\mathbf{w})$$
其中$\mathbf{x}$为状态向量，$\mathbf{u}$为受噪声影响的IMU测量，$\mathbf{w}$为高斯白噪声扰动
$$\mathbf{x}=\left[\begin{array}{c}\mathbf{p}\\ \mathbf{v}\\ \mathbf{q}\\ {\mathbf{b}}_{\mathbf{a}}\\ {\mathbf{b}}_{\mathbf{\omega }}\\ \mathbf{g}\end{array}\right]\mathbf{u}=\left[\begin{array}{c}{\mathbf{a}}_m-{\mathbf{n}}_{\mathbf{a}}\\ {\mathbf{\omega }}_m-{\mathbf{n}}_{\mathbf{\omega }}\end{array}\right]\mathbf{w}=\left[\begin{array}{c}{\mathbf{n}}_{\mathbf{ba}}\\ {\mathbf{n}}_{\mathbf{b}\mathbf{\omega }}\end{array}\right]$$

The nominal-state kinematics

系统的名义态运动学方程对应于无噪声和扰动的系统模型。

$$\begin{array}{rl}\dot{\bar{\mathbf{p}}}& =\bar{\mathbf{v}}\\ \dot{\bar{\mathbf{v}}}& =\bar{\mathbf{R}}\left({\mathbf{a}}_m-{\bar{\mathbf{b}}}_a\right)+\bar{\mathbf{g}}\\ \dot{\bar{\mathbf{q}}}& =\frac{1}{2}\bar{\mathbf{q}}\otimes \left({\mathbf{\omega }}_m-{\bar{\mathbf{b}}}_{\mathbf{\omega }}\right)\\ {\dot{\bar{\mathbf{b}}}}_a& =0\\ {\dot{\bar{\mathbf{b}}}}_{\mathbf{\omega }}& =0\\ \dot{\bar{\mathbf{g}}}& =0\end{array}\text{\hspace{1em}(nominal state)}$$

The error-state kinematics

由$\mathbf{x}=\bar{\mathbf{x}}+\delta \mathbf{x}⟹\delta \mathbf{x}=\mathbf{x}-\bar{\mathbf{x}}$，其中+,-代指compose和decompose，根据变量类型采取不同的compose方式， 对于四元数
$$\mathbf{q}=\bar{\mathbf{q}}\otimes \delta \mathbf{q}$$
为了简便起见，我们将角速度分成大信号量项$\bar{\mathbf{\omega }}\triangleq {\mathbf{\omega }}_m-{\bar{\mathbf{b}}}_{\mathbf{\omega }}$和小信号量项$\delta \mathbf{\omega }=-\delta {\mathbf{b}}_{\mathbf{\omega }}-{\mathbf{n}}_{\mathbf{\omega }}$
$$\mathbf{\omega }=\bar{\mathbf{\omega }}+\delta \mathbf{\omega }$$
于是
$$\begin{array}{rl}\dot{\mathbf{q}}=\dot{\bar{\mathbf{q}}}\otimes \delta \mathbf{q}+\bar{\mathbf{q}}\otimes \dot{\delta \mathbf{q}}& =\frac{1}{2}\mathbf{q}\otimes \left({\mathbf{\omega }}_m-{\mathbf{b}}_{\mathbf{\omega }}-{\mathbf{n}}_{\mathbf{\omega }}\right)\\ \frac{1}{2}\bar{\mathbf{q}}\otimes \left({\mathbf{\omega }}_m-{\bar{\mathbf{b}}}_{\mathbf{\omega }}\right)\otimes \delta \mathbf{q}+\bar{\mathbf{q}}\otimes \dot{\delta \mathbf{q}}& =\frac{1}{2}\bar{\mathbf{q}}\otimes \delta \mathbf{q}\otimes \left({\mathbf{\omega }}_m-{\mathbf{b}}_{\mathbf{\omega }}-{\mathbf{n}}_{\mathbf{\omega }}\right)\\ ⟹2\dot{\delta \mathbf{q}}& =\delta \mathbf{q}\otimes \mathbf{\omega }-\bar{\mathbf{\omega }}\otimes \delta \mathbf{q}\\ & =\left([\mathbf{\omega }{]}_R-[\bar{\mathbf{\omega }}{]}_L\right)\delta \mathbf{q}\\ & =\left[\begin{array}{cc}0& -{\left(\mathbf{\omega }-\bar{\mathbf{\omega }}\right)}^T\\ \mathbf{\omega }-\bar{\mathbf{\omega }}& -{\left(\mathbf{\omega }+\bar{\mathbf{\omega }}\right)}^{\wedge }\end{array}\right]\delta \mathbf{q}\\ & =\left[\begin{array}{cc}0& -\delta {\mathbf{\omega }}^T\\ \delta \mathbf{\omega }& -{\left(2\overline{\mathbf{\omega }+\delta \mathbf{\omega }}\right)}^{\wedge }\end{array}\right]\delta \mathbf{q}\end{array}$$
对$\delta \mathbf{q}$进行小量近似，$\delta \mathbf{q}=\left[\begin{array}{c}\cos (\Vert \delta \mathbf{\theta }\Vert /2)\\ \frac{\delta \mathbf{\theta }}{\Vert \delta \mathbf{\theta }\Vert }\sin (\Vert \delta \mathbf{\theta }\Vert /2)\end{array}\right]\approx \left[\begin{array}{c}1\\ \delta \mathbf{\theta }/2\end{array}\right]+O(\Vert \delta \mathbf{\theta }{\Vert }^2)$，于是
$$\begin{array}{rl}\left[\begin{array}{c}0\\ \dot{\delta \mathbf{\theta }}\end{array}\right]& =[\begin{array}{cc}0& -\delta {\mathbf{\omega }}^T\\ \delta \mathbf{\omega }& -{\left(2\mathbf{\omega }+\delta \mathbf{\omega }\right)}^{\wedge }\end{array}\\ ⟹\dot{\delta \mathbf{\theta }}& \end{array}$$