What is 3Q3? A 3Q3 problem aims to solve three (coupled) quadratic equations for three variables. $$ Q_i(x,y,z)=0\quad\textrm{for}\quad i=1,2,3 $$ where $Q_i$ is a quadratic polynomial of $x$ , $y$ and $z$ . Examples Forward kinematics of spherical parallel manipulator: Given $\bm{w}_i$ and $\bm{v}_i$ find rotation $\mat{R}$ such that $$ \bm{w}_i^\top \mat{R}\bm{v}_i=\cos\alpha_2 \quad\textrm{for}\quad i=1,2,3 $$ This problem becomes 3Q3 if rotation $\mat{R}$ is parametrized using Cayley transformation $$ \mat{R}=\frac{1}{ 1 + |\bm{r}|^2} \begin{bmatrix} 1+x^2-y^2-z^2&2xy-2z&2y+2xz\nl 2 z + 2 x y &1-x^2+y^2-z^2&2 y z − 2 x\nl 2 x z − 2 y &2 x + 2 y z &1-x^2-y^2+z^2 \end{bmatrix} $$

Bias Calibration Incremental encoders are ideal for various industries due to their small size and excellent performance. Bias calibration is a necessary procedure in order to make sense of the readings from incremental encoders. In this article, we study the bias calibration problem for spherical parallel manipulator. Let $\hat{\bm{q}}$ be the measured encoder vector and $\hat{\mat{R}}$ be the measured end-effector pose. The bias calibration problem looks for a bias vector $\bm{b}$ such that

Kinematic Model of SPM SPM is described by the relation between encoder vector $\bm{q}\in\mathbb{R}^3$ and three platform vectors $\bm{v}_1$, $\bm{v}_2$ and $\bm{v}_3$ . $$ \begin{aligned} \bm{w}_i\cdot \bm{v}_i& = \cos\alpha_2,&&~~i=1,2,3\nl \bm{v}_i\cdot \bm{v}_j& = \cos\alpha_3,&&~~i\neq j\nl \bm{v}_i\cdot \bm{v}_i& = 1,&&~~i=1,2,3 \end{aligned}\tag{0} $$ Platform orientation is defined by the unique rotation matrix $\mat{R}$ such that $$ \mat{R}\bm{v}_i=\underline{\bm{v}}_i~,~~i=1,2,3 $$ where $\underline{\bm{v}}_i$ are the home configuration of $\bm{v}_i$. The forward kinematics (FK) problem reads

动力学方程 设 $\bm{\omega}(t)$ 为惯性测量单元自身坐标系下测量的旋转读数， $\mat{R}(t)$ 为世界坐标系下惯性测量单元的旋转矩阵。 我们应该如何对刚体动力学中描述旋转的方程 $$ \dd{\mat{R}}{t}=\mat{R}(t)\bm{\omega}(t)^\wedge~,\quad \mat{R}(0)=\mat{R}_0\tag{1} $$ 做时间积分得到 $\mat{R}(t)$ ？ 精确解析解 很多文章和笔记上来就直接假设旋转读数 $\bm{\omega}(t)$ 在很短的时间段 $[0,t]$ 里可以近似为常量，然后直接得出类似于 $\mat{R} \exp(\bm{\omega}^\wedge(0))$ 的近似解。 其实这个常微分方程在短时间内是存在精确的解析解的。我们从之前推导出的关于指数映射导数的公式出发， $$ \dd{\exp{\mat{x}(t)}}{t}=\exp{\mat{x}}\frac{\mathbb{I}-\exp{(-\mathrm{ad}_{\mat{x}})}}{\mathrm{ad}_{\mat{x}}} \dd{\mat{x}}{t} $$ 两边左乘常量矩阵 $\mat{R}_0$ 可得 $$ \dd{\mat{R}_0\exp{\mat{x}(t)}}{t}= \mat{R}_0\exp{\mat{x}(t)} \frac{\mathbb{I}-\exp{(-\mathrm{ad}_{\mat{x}})}}{\mathrm{ad}_{\mat{x}}} \dd{\mat{x}}{t}~. $$ 换句话说如果我们能解出这个关于 $\mat{x}(t)$ 的常微分方程 $$ \frac{\mathbb{I}-\exp{(-\mathrm{ad}_{\mat{x}})}}{\mathrm{ad}_{\mat{x}}} \dd{\mat{x}}{t}=\bm{\omega}(t)^\wedge \Leftrightarrow \dd{\mat{x}}{t}=\frac{\mathrm{ad}_{\mat{x}}}{\mathbb{I}-\exp{(-\mathrm{ad}_{\mat{x}})}} \bm{\omega}(t)^\wedge $$ 那么 $\mat{R}(t)=\mat{R}_0\exp{\mat{x}(t)}$ 就是方程 $(1)$ 满足初始条件的解。 通过简单的换元 $\mat{y}=-\mat{x}$ 以及 $\mat{a}(t)=-\bm{\omega}(t)^\wedge$ 可得无穷级数常微分方程， $$ \dd{\mat{y}}{t}=\frac{\mathrm{ad}_{\mat{y}}}{\exp{(\mathrm{ad}_{\mat{y}})}-\mathbb{I}}\mat{a}(t) =\sum_{k=0}^\infty\frac{B_k}{k!}\mathrm{ad}_{\mat{y}}^k\mat{a}(t)~.\tag{2} $$ 第二个等式的推导是比较复杂的，因为并不是单变量而是矩阵方程的级数展开，其中用到了伯努利数 Bernoulli number $B_k$ 做近似。 常微分方程 $(2)$ 的解可以通过皮卡逐次逼近法 Picard successive approximation method 来近似。经典的皮卡逐次逼近法是通过迭代

球面线性插值的弊端 球面线性插值 spherical linear interpolation 主要用于 在两个表示旋转变换比如 $\mat{X}_0$ 和 $\mat{X}_1$ 之间的平滑差值（注意只有两个）。思路很简单：利用对数映射把插值的过程转移到李代数（也是线性空间）里操作。李群上构造的插值曲线 $\mat{P}_0(0\le t\le 1)$ 满足 $\mat{P}_0(0)=\mat{X}_0$ 以及 $\mat{P}_0(1)=\mat{X}_1$ 即可。 我们可以选择直接插值旋转 $$ \mat{P}_0(t)=\mathrm{Exp}((1-t)\mathrm{Log}(\mat{X}_0)+t \mathrm{Log}(\mat{X}_1))~, $$ 或者插值旋转增量 $$ \mat{P}_0(t)=\mat{X}_0\mathrm{Exp}(t \mathrm{Log}(\mat{X}_0^{-1}\mat{X}_1))~. $$ 在这篇文章里我们采用前者。 推广到多个旋转变换的线性插值非常简单。 设 $\mat{x}_k=\mathrm{Log}(\mat{X}_k)$ 。对于一组旋转矩阵的离散数据 $\lbrace\mat{X}_0,\mat{X}_1,…,\mat{X}_N\rbrace$ ，我们先 计算出相应的李代数 $\lbrace\mat{x}_0,\mat{x}_1,…,\mat{x}_N\rbrace$ ， 然后引入一组关节点参数 $\lbrace t_0 ,t_1,…,t_N\rbrace$ 。关节点参数可采用最简单的 $t_k=k$ 或者李代数的累加弦长 accumulated chord length 。 接着我们定义局部线性插值函数为 $$ \mat{P}_k(t)=\mathrm{exp}\Big(\frac{t-t_{k+1}}{t_k-t_{k+1}}\mat{x}_{k}+\frac{t-t_{k}}{t_{k+1}-t_{k}} \mat{x}_{k+1}\Big)~. $$ 继而得出全局分段线性插值函数为 $$ \mat{S}(t)=\left\lbrace \begin{aligned} &\mat{P}_0(t)~,&&t\in[t_0,t_1]\nl &\mat{P}_1(t)~,&&t\in [t_1,t_2]\nl &\quad\vdots&&\nl &\mat{P}_{N-1}(t)~,&&t\in[t_{N-1},t_{N}]\nl \end{aligned}\right. $$ 乍看之下，上述球面线性插值和欧氏空间内的一般插值方法没有什么太大区别。 但其实隐藏着一个致命的问题。就拿 $\mat{R}\in\mathrm{SO}(3)$ 为例，其对数映射的具体形式为，

In multivariate MLE problems, logarithm of matrix determinant often appears in the normalization factor. Its concavity matters during the optimization stage. In this article, the following statement is proved, $\mat{X}\mapsto\ln\det\mat{X}$ is a concave function of all positive definite matrices $\mat{X}$ . Schur Decomposition Schur decomposition exists for a square matrix $$ \mat{A}=\mat{Q}\mat{U}\mat{Q}^\dagger $$ where $\mat{Q}$ is an unitary matrix and $\mat{U}$ is an upper triangular matrix. If $\mat{A}$ is also positive definite, then the square root of $\mat{A}$ is well defined,

Let $\mat{X}$ be a random variable in a Lie group $\mathcal{M}$. Assuming $\mat{X}\in \mathcal{M}$ have small fluctuations near a mean element $\bar{\mat{X}}$. In robotics, $\mathcal{M}$ is mostly likely a curved manifold not an Euclidean space, where the usual notion of plus and minus in classical probability theory doesn’t apply. For example, probability density $p(x)\dx{x}$ makes sense for a standard one-dimensional distribution while the meaning of $p(\mat{X})\dx{\mat{X}}$ definitely needs some clarification.

Continuous-Time Kalman-Bucy Filter -- 引言 时间离散卡曼滤波器的推导在很多地方都能找到，也非常好理解。 这里我们对不太常见的时间连续系统的卡曼-波希滤波器 Kalman-Bucy filter 做一个推导。 常见的推导方法对于随着时间连续变化的物理量 采用了类似于格林函数的核卷积表示， 数学技巧性比较高。这里我们从最优控制论 optimal control theory 的角度， 把滤波器的设计转换为一个优化问题，同时用到的数学工具也相对基础。 时间连续线性系统 我们用 $\bm{x}$ 表示系统状态 state ， $\bm{z}$ 表示对系统状态的观测 observation 。 随着时间变化的线性系统一般可分为 连续状态 $\bm{x}(t)$ 和连续观测 $\bm{z}(t)$ 连续状态 $\bm{x}(t)$ 和离散观测 $\bm{z}_k$ 离散状态 $\bm{x}_k$ 和离散观测 $\bm{z}_k$ 大多数文章里推导的都是离散-离散的卡曼滤波。 如果系统状态和对状态的观测是时间连续的，则可以通过线性随机微分方程对系统进行定量描述， $$ \left\lbrace \begin{aligned} \dd{\bm{x}}{t}&=\mat{A}(t) \bm{x}(t)+\mat{G}(t)\bm{w}(t)~,\nl \bm{z}(t)&=\mat{H}(t) \bm{x}(t)+\bm{v}(t)~. \end{aligned}\right.\tag{1} $$ $\mat{A}(t)$ 是描述系统状态瞬时增量的线性强迫 forcing ， $\mat{H}(t)$ 代表从状态空间映射到观测空间的线性观测模型 observation model 。 系统还可以加入控制器 $\mat{B}(t)\bm{u}(t)$ 对状态进行修正，在这里我们不作讨论。 状态和观测的噪音是零均值 zero-mean 和不自相关 uncorrelated 的随机变量（白噪音），