最优加权平均估计

假设有两只精度不一样的电压表\(M_1\)和\(M_2\),他们的量测误差都服从零均值的正态分布,方差分别为\(\sigma_1^2\)和\(\sigma_2^2\)。若使用这两只表对同一真值为\(u\)的电压各测量一次,两次测量之间互不影响,读数分别为\(u_1\)和\(u_2\)。试通过这两次测量估计电压真值。 即:

\[ \left\{\begin{array}{l}{u_{1}=u+\varepsilon_{1}} \\ {u_{2}=u+\varepsilon_{2}}\end{array}\right. \\ \tag{1.1.1}\label{1.1.1} \]

并且:

\[ \varepsilon_{1} \sim N\left(0, \sigma_{1}^{2}\right), \varepsilon_{2} \sim N\left(0, \sigma_{2}^{2}\right), E\left[\varepsilon_{1} \varepsilon_{2}\right]=0 \\ \tag{1.1.2}\label{1.1.2} \]

为了确定电压的真值,最朴素的想法是将这两次电压表的测量值进行简单的算术平均或者加权平均作为真值的估计。 即:

\[ \hat{u}_{I}=\frac{1}{2}\left(u_{1}+u_{2}\right) \tag{1.1.3}\label{1.1.3} \]

从统计学的观点,估计\(\hat{u}_i\)的误差(方差)为:

\[ D\left[\hat{u}_{I}\right]=D\left[\frac{1}{2}\left(u_{1}+u_{2}\right)\right]=\frac{1}{4}\left(D\left[u_{1}\right]+D\left[u_{2}\right]\right)=\frac{1}{4}\left(\sigma_{1}^{2}+\sigma_{2}^{2}\right) \tag{1.1.4}\label{1.1.4} \]

若不等加权平均处理,去\(M_1\)的权重为\(1 - a\),\(M_2\)的权重为\(a\)。则:

\[ \hat{u}_{II}=(1-\alpha) u_{1}+\alpha u_{2} \tag{1.1.5}\label{1.1.5} \]

此时估计的方差为:

\[ \begin{aligned} D\left[\hat{u}_{II}\right] &\left.=D\left[(1-\alpha) u_{1}+\alpha u_{2}\right)\right] \\ &=(1-\alpha)^{2} D\left[u_{1}\right]+\alpha^{2} D\left[u_{2}\right]=(1-\alpha)^{2} \sigma_{1}^{2}+\alpha^{2} \sigma_{2}^{2} \end{aligned} \tag{1.1.6}\label{1.1.6} \]

易得此时方差为二次函数,令导数为零可得到最小值:

\[ \frac{\mathrm{d} D\left[\hat{u}_{I I}\right]}{\mathrm{d} \alpha}= -2(1-\alpha) \sigma_{1}^{2}+2 \alpha \sigma_{2}^{2} =0 \tag{1.1.7}\label{1.1.7} \]

可得最优加权因子:

\[ \alpha=\frac{\sigma_{1}^{2}}{\sigma_{1}^{2}+\sigma_{2}^{2}} \tag{1.1.8}\label{1.1.8} \]

对应最小方差为:

\[ D\left[\hat{u}_{II}\right]=(1-\alpha)^{2} \sigma_{1}^{2}+\alpha^{2} \sigma_{2}^{2}=\frac{\sigma_{1}^{2} \sigma_{2}^{2}}{\sigma_{1}^{2}+\sigma_{2}^{2}} \\ \tag{1.1.9}\label{1.1.9} \]

因此,考虑不同量测信息源的误差统计特性,对所有可用信息源进行优化组合,有可能获得被测对象更准确的估计,甚至是在一定准侧下的所谓最优估计。正是Kalman滤波所要解决的问题。

Kalman Filter

加权平均估计方法仅需建立在量测方程基础上,但是如果被估计对象不能被直接测量,就难以进行有效的估计。这里从一维简单的模型开始,推导出Kalman Filter。

一维随机系统的状态空间模型与基本假设:

假设一维离散随机系统的状态空间模型:

\[ \left\{\begin{array}{l}{x_{k}=\phi_{k / k-1} x_{k-1}+B_{k-1} w_{k-1}} \\ {y_{k}=H_{k} x_{k}+v_{k}}\end{array}\right. \tag{2.1.1}\label{2.1.1} \]

其中:\(x_k\)是系统状态,\(y_k\)是量测,\(\phi_{k / k-1}, B_{k - 1}, H_k\)是已知的系统结构参数,\(w_{k - 1}\)是状态激励噪声或称系统噪声,\(v_k\)是量测噪声。他们都是零均值的高斯白噪声序列,且两个白噪声之间互不相关,即:

\[ \left\{\begin{array}{ll}{E\left[w_{k}\right]=0,} & {E\left[w_{j} w_{k}^{\mathrm{T}}\right]=Q_{k} \delta_{j k}} \\ {E\left[v_{k}\right]=0,} & {E\left[v_{j} v_{k}^{\mathrm{T}}\right]=R_{k} \delta_{j k}} \\ {E\left[w_{j} v_{k}^{\mathrm{T}}\right]=0}\end{array}\right. \\ \tag{2.1.2}\label{2.1.2} \]

Kalman 滤波公式

通过某种手段获得一个量测样本序列\(\{y_k\}\),如何求得系统状态序列\(\{x_k\}\)?显然\(\{x_k\}\)是随机过程的一个轨迹,除非直接测量并且不存在任何量测误差,否则不能精确求解。而只能根据某种准则或指标函数给出\(\{x_k\}\)的估计值。常用的准则是使估计误差统计均方值(Mean Square)最小,称为均方误差(Mean Square Error, MSE),进一步,如果估计的误差是零均值的(无偏估计),则均方误差与方差相等也称最小方差估计(Minimum variance Estimation)。

  1. 记\(k - 1\)时刻状态\(x_{k - 1}\)的估计值为\(\hat{x}_{k - 1}\), 估计误差为:

    \[ e_{x, k-1}=x_{k-1}-\hat{x}_{k-1} \tag{2.1.3}\label{2.1.3} \]

    记状态估计\(\hat{x}_{k - 1}\)的均方误差为:

    \[ P_{x, k-1}=E\left[e_{x, k-1} e_{x, k-1}^{\mathrm{T}}\right] \tag{2.1.4}\label{2.1.4} \]
  2. 记状态一步预测及预测误差:

    \[ \begin{aligned} \widehat{x}_{k / k-1}&=E\left[\phi_{k / k-1} \widehat{x}_{k-1}+B_{k-1} w_{k-1}\right]=\phi_{k / k-1} \hat{x}_{k-1} \\ e_{x, k/k-1} &=x_{k}-\hat{x}_{k/k-1}=\left(\phi_{k / k-1} x_{k-1}+B_{k-1} w_{k-1}\right)-\phi_{k / k-1} \hat{x}_{k-1} \\ &=\phi_{k / k-1}\left(x_{k-1}-\hat{x}_{k-1}\right)+B_{k-1} w_{k-1}=\phi_{k k-1} e_{x, k-1}+B_{k-1} w_{k-1} \end{aligned} \tag{2.1.5}\label{2.1.5} \]

    容易判断\(e_{x,k-1}\)和\(w_{k-1}\)两者之间互不相关,记状态一步预测的均方误差:

    \[ P_{x, k / k-1}=E\left[e_{x, k / k-1} e_{x, k / k-1}^{\mathrm{T}}\right]=\phi_{k / k-1}^{2} P_{x, k-1}+B_{k-1}^{2} Q_{k-1} \tag{2.1.6}\label{2.1.6} \]
  3. 记量测一步预测及预测误差:

    \[ \begin{aligned} \hat{y}_{k / k-1}&=E\left[H_{k} \hat{x}_{k / k-1}+v_{k}\right]=H_{k} \hat{x}_{k / k-1} \\ e_{y, k / k-1}&=y_{k}-\hat{y}_{k / k-1}=\left(H_{k} x_{k}+v_{k}\right)-H_{k} \hat{x}_{k / k-1}=H_{k} e_{x, k / k-1}+v_{k} \end{aligned} \tag{2.1.7}\label{2.1.7} \]

    容易判断\(e_{x,k-1}\)和\(v_{k-1}\)两者之间互不相关,记量测一步预测的均方误差:

    \[ P_{y, k | k-1}=E\left[e_{y, k | k-1} e_{y, k | k-1}^{\mathrm{T}}\right]=H_{k}^{2} P_{x, k / k-1}+R_{k} \tag{2.1.8}\label{2.1.8} \]
  4. 记状态一步预测误差\(e_{x, k/k-1}\)和量测一步预测误差\(e_{y,k/k-1}\)之间的二阶混合原点矩:

    \[ P_{x y, k / k-1}=E\left[e_{x, k / k-1} e_{y, k / k-1}^{\mathrm{T}}\right]=E\left[e_{x, k / k-1}\left(H_{k} e_{x, k / k-1}-v_{k}\right)^{\mathrm{T}}\right]=P_{x, k / k-1} H_{k} \tag{2.1.9}\label{2.1.9} \]

    当\(k-1\)时刻的状态估计\(\hat{x}_{k - 1}\)无偏时,即\(E[e_{x,k-1}] = 0\),根据(2.1.5)(2.1.7)有\(E\left[e_{x, k / k-1}\right]=E\left[e_{y, k / k-1}\right]=0\)所以\(P_{xy,k/k-1}\)就等于\(e_{x,k/k-1}\)与\(e_{与,k/k-1}\)之间的二阶混合中心矩(协方差),即\(P_{x y, k / k-1}=\operatorname{Cov}\left(e_{x, k / k-1}, e_{y, k / k-1}\right)\)。

  5. 记k时刻状态\(x_k\)的估计值\(\hat{x}_k\),估计误差为:

    \[ e_{x, k}=x_{k}-\hat{x}_{k} \tag{2.1.10}\label{2.1.10} \]

    记状态估计\(\hat{x}_k\)的均方误差为:

    \[ P_{x, k}=E\left[e_{x, k} e_{x, k}^{\mathrm{T}}\right] \tag{2.1.11}\label{2.1.11} \]

假设已知前一时刻状态估计\(\hat{x}_{k -1}\)并且已知它的估计误差均值\(E[e_{x,k-1}] = 0\)和均方误差\(P_{x,k-1}\),当新的观测\(y_k\)到来时,推导当前时刻的状态估计\(\hat{x}_k\)、状态估计的误差均值\(E[e_{x,k}]\)和均方误差\(P_{x,k}\)。

由于系统噪声\(w_{k-1}\)和量测噪声\(v_k\)的存在,并受到最优加权平均的思路启发。因此可令当前时刻的状态估计:

\[ \widehat{x}_{k}=\left(1-\alpha_{k}\right) \phi_{k / k-1} \widehat{x}_{k-1}+\alpha_{k} \frac{1}{H_{k}} y_{k} \tag{2.2.1}\label{2.2.1} \]

其中\(\alpha_k\)是待定加权因子,将(2.2.1)带入(2.1.10),的k时刻的估计误差:

\[ \begin{aligned} e_{x, k} &=x_{k}-\left[\left(1-\alpha_{k}\right) \phi_{k / k-1} \hat{x}_{k-1}+\alpha_{k} \frac{1}{H_{k}} y_{k}\right] \\ &=x_{k}-\left[\left(1-\alpha_{k}\right) \phi_{k / k-1} \hat{x}_{k-1}+\alpha_{k} \frac{1}{H_{k}}\left(H_{k} x_{k}+v_{k}\right)\right] \\ &=-\left(1-\alpha_{k}\right) \phi_{k / k-1} \hat{x}_{k-1}+\left(1-\alpha_{k}\right) x_{k}-\alpha_{k} \frac{1}{H_{k}} v_{k} \\ &=\left(1-\alpha_{k}\right) e_{x, k / k-1}-\alpha_{k} \frac{1}{H_{k}} v_{k} \end{aligned} \tag{2.2.2}\label{2.2.2} \]

在上式中,由于\(E[e_{x,k/k-1}] = 0\)和\(E[v_k] = 0\),所以有\(E[e_{x,k}] = 0\),可知估计\(\hat{x_k}\)是无偏的。又由于\(e_{x,k/k-1}\)和\(v_k\)两者之间互不相关,所以状态估计\(\hat{x_k}\)的均方误差为:

\[ P_{x, k}=E\left[e_{x, k} e_{x, k}^{\mathrm{T}}\right]=\left(1-\alpha_{k}\right)^{2} P_{x, k / k-1}+\alpha_{k}^{2} \frac{1}{H_{k}^{2}} R_{k} \tag{2.2.3}\label{2.2.3} \]

为了使\(P_{x,k}\)取最小值,将(2.2.3)对待定参数\(\alpha_k\)求导并令其等于零:

\[ \frac{\mathrm{d} P_{x, k}}{\mathrm{d} \alpha_{k}}=-2\left(1-\alpha_{k}\right) P_{x, k / k-1}+2 \alpha_{k} \frac{1}{H_{k}^{2}} R_{k}=0 \tag{2.2.4}\label{2.2.4} \]

因此,求得最优加权因子:

\[ \alpha_{k}=\frac{H_{k}^{2} P_{x, k / k-1}}{H_{k}^{2} P_{x, k / k-1}+R_{k}}=\frac{H_{k} P_{x y, k / k-1}}{P_{y, k / k-1}} \tag{2.2.5}\label{2.2.5} \]

不难看出\(\alpha_k\)的取值范围是\(\alpha_k \in [0,1]\)。 若\(\alpha_k = 0\)则量测缺失,只能通过\(\hat{x}_{k-1}\)递推\(\hat{x}_k\)。 若\(\alpha_k = 1\)则相当于直接通过量测求解\(\hat{x}_k\),这正是传统加权平均估计的做法。

结合(2.2.5),(2.1.6)得:

\[ \alpha_{k}=\frac{H_{k}^{2}\left(\phi_{k / k-1}^{2} P_{x, k-1}+B_{k-1}^{2} Q_{k-1}\right)}{H_{k}^{2}\left(\phi_{k / k-1}^{2} P_{x, k-1}+B_{k-1}^{2} Q_{k-1}\right)+R_{k}} \\ \tag{2.2.6}\label{2.2.6} \]

由此可以看出。 若量测噪声\(R_k\)越大,表示量测可靠性低,加权因子\(\alpha_k\)越小,量测\(y_k\)对状态估计\(\hat{x}_k\)的贡献越小,\(\hat{x}_{k-1}\)对\(\hat{x}_k\)的贡献越大。 若系统噪声\(Q_{k-1}\)(或者前一时刻估计误差\(P_{x,k-1}\))越大,表示预测的可靠性低,则\(\alpha_k\)越大,量测\(y_k\)对状态估计\(\hat{x}_k\)的贡献越大,\(\hat{x}_{k-1}\)对\(\hat{x}_k\)的贡献越小。

进一步,若记\(K_k = \alpha_k/H_k\),\(G_k = 1 - K_kH_k\)。则(2.2.1)(2.2.3)(2.2.5)可整理为:

\[ \left\{\begin{array}{l} {K_{k}=P_{x y, k / k-1} P_{y, k / k-1}^{-1}} \\ {\hat{x}_{k}=G_{k} \hat{x}_{k / k-1}+K_{k} y_{k}} \\ {P_{x, k}=G_{k}^{2} P_{x, k / k-1}+K_{k}^{2} R_{k}} \end{array}\right. \tag{2.2.7}\label{2.2.7} \]

便得出完整标量Kalman滤波公示,主要包含这么几个部分:一是在状态估计均方误差最小准则下的求解加权系数;儿时线性加权平均状态估计;三是状态估计的均方误差跟新,为下一次估计做准备。其中,\(P_{x,k}\)代表了状态估计的精度或可靠性,和状态估计\(\hat{x}_k\)同样重要,也是滤波器输出的组成部分。

进一步,通过改写(2.2.6)可将状态估计公示改写成‘预测+校正’的形式:

\[ \hat{x}_{k}=\hat{x}_{k / k-1}+K_{k}\left(y_{k}-\hat{y}_{k / k-1}\right) \tag{2.2.8}\label{2.2.8} \]

可以将状态预测\(\hat{x}_{k/k-1}\)和状态估计\(\hat{x}_k\)分别称为先验估计和后验估计,将实际量测与量测预测之间的残差\(e_{y,k/k-1} = y_k - \hat{y}_k/k-1\)称为第k次量测获得到的新息(innovation)。新息即由最近时刻量测携带的关于状态的新信息,用以修正先验估计。因此(2.2.8)的直观含义是利用新息\(e_{y,k/k-1}\)对先验估计\(\hat{x}_{k/k-1}\)进行校正得到后验估计\(\hat{x}_k\)。\(K_k\)是新息的利用权重系数,也称Kalman增益。显然,若系统噪声\(Q_k\)越大则增益\(K_k\)越大,表示先验估计不太准确,新息就利用的越多;若量测噪声\(R_k\)越大则增益\(K_k\)越小,表示先验估计比较准确,新息就利用的越少。 若将(2.2.7)中均方误差更新公示展开整理:

\[ \begin{aligned} P_{x, k} &=\left(1-K_{k} H_{k}\right)^{2} P_{x, k / k-1}+K_{k}^{2} R_{k} \\ &=\left(1-K_{k} H_{k}\right) P_{x, k / k-1}-K_{k} H_{k} P_{x, k / k-1}+K_{k}^{2} H_{k}^{2} P_{x, k /k-1}+K_{k}^{2} R_{k} \\ &=\left(1-K_{k} H_{k}\right) P_{x, k / k-1}-K_{k} P_{x y, k / k-1}+K_{k}^{2} P_{y, k / k-1} \\ &=\left(1-K_{k} H_{k}\right) P_{x, k / k-1} \end{aligned} \tag{2.2.9}\label{2.2.9} \]

再将\(H_{k} P_{x, k / k-1}=P_{x y, k / k-1}=K_{k} P_{y, k / k-1}\)代入得:

\[ P_{x, k}=P_{x, k / k-1}-K_{k}^{2} P_{y, k / k-1} \tag{2.2.10}\label{2.2.10} \]

所以Kalman滤波公示写成经典表示形式:

\[ \left\{\begin{array}{l} {K_{k}=P_{x y, k / k-1} P_{y, k / k-1}^{-1}} \\ {\hat{x}_{k}=\hat{x}_{k / k-1}+K_{k}\left(y_{k}-\hat{y}_{k / k-1}\right)} \\ {P_{x, k}=P_{x, k / k-1}-K_{k}^{2} P_{y, k / k-1}} \end{array}\right. \tag{2.2.11}\label{2.2.11} \]

其中:

\[ \hat{x}_{k / k-1}=\phi_{k / k-1} \hat{x}_{k-1}, \hat{y}_{k / k-1}=H_{k} \hat{x}_{k / k-1}\\ P_{x, k / k-1}=\phi_{k / k-1}^{2} P_{x, k-1}+B_{k-1}^{2} Q_{k-1}, P_{y, k / k-1}=H_{k}^{2} P_{x, k / k-1}+R_{k}, P_{x y, k k-1}=H_{k} P_{x, k / k-1} \\ \tag{2.2.12}\label{2.2.12} \]

习惯上常将\(\hat{x}_{k / k-1}=\phi_{k / k-1} \hat{x}_{k-1}\)和\(P_{x, k / k-1}=\phi_{k / k-1}^{2} P_{x, k-1}+ B_{k-1}^2Q_{k-1}\)两个公式称为时间更新(预测过程),不论量测是否有效,滤波递推每一时刻都在进行时间更新;而剩余的其他公示称为量测更新(校正过程),只有量测到来有效时才需计算。 设\(\phi_{k / k-1} \geqslant 1\),从公式\(P_{x, k / k-1}=\phi_{k / k-1}^{2} P_{x, k-1}+B_{k-1}^{2} Q_{k-1}\)看出,由于系统噪声\(Q_{k-1}\)的影响,状态预测的先验估计误差不断变大,当量测到来时,均方误差更新公式\(P_{x, k}=P_{x, k / k-1}-K_{k}^{2} P_{y, k / k-1}\)显示,经过量测新息校正之后的状态后验估计误差逐渐减小。因此Kalman滤波的状态估计误差在系统噪声和新息的共同作用下进行调整并达到动态平衡,只有持续的提供量测才能将状态估计误差控制在较低的水平上。

向量Kalman滤波

对于\(n\)维随机系统状态空间模型:

\[ \left\{\begin{array}{l} {\boldsymbol{x}_{k}=\boldsymbol{\Phi}_{k / k-1} \boldsymbol{x}_{k-1}+\boldsymbol{B}_{k-1} \boldsymbol{w}_{k-1}} \\ {\boldsymbol{y}_{k}=\boldsymbol{H}_{k} \boldsymbol{x}_{k}+\boldsymbol{v}_{k}} \end{array}\right. \tag{2.3.1}\label{2.3.1} \]

式中:\(\boldsymbol{x}_k\)是\(n\times 1\)维的状态向量;\(\boldsymbol{y}_k\)是\(m \times 1\)维的量测向量;\(\boldsymbol{\phi}_{k/k-1}\),\(\boldsymbol{B}_{k-1}\),\(\boldsymbol{H}_k\)是已知的系统结构参数,分别称为\(n \times n\)维的状态一步转移矩阵,\(n \times l\)维的系统噪声分配矩阵,\(m \times n\)维的量测矩阵;\(\boldsymbol{w}_{k-1}\)是\(l \times 1\)维的系统噪声向量,\(\boldsymbol{v}_k\)是\(m \times 1\)维的量测噪声向量,两者都是零均值的高斯白噪声向量序列,且它们之间互不相关,即:

\[ \left\{\begin{array}{ll} {E\left[\boldsymbol{w}_{k}\right]=\mathbf{0},} & {E\left[\boldsymbol{w}_{j} \boldsymbol{w}_{k}^{\mathrm{T}}\right]=\boldsymbol{Q}_{k} \delta_{j k}} \\ {E\left[\boldsymbol{v}_{k}\right]=\mathbf{0},} & {E\left[\boldsymbol{v}_{j} \boldsymbol{v}_{k}^{\mathrm{T}}\right]=\boldsymbol{R}_{k} \boldsymbol{\delta}_{j k}} \\ {E\left[\boldsymbol{w}_{j} \boldsymbol{v}_{k}^{\mathrm{T}}\right]=\mathbf{0}} \end{array}\right. \tag{2.3.2}\label{2.3.2} \]

仿照标量Kalman滤波公示推导思路,可容易获得向量Kalman滤波公式:

\[ \left\{\begin{array}{l} {\boldsymbol{K}_{k}=\boldsymbol{P}_{x y, k / k-1} \boldsymbol{P}_{y, k / k-1}^{-1}} \\ {\hat{\boldsymbol{x}}_{k}=\hat{\boldsymbol{x}}_{k / k-1}+\boldsymbol{K}_{k}\left(\boldsymbol{y}_{k}-\hat{\boldsymbol{y}}_{k / k-1}\right)} \\ {\boldsymbol{P}_{x, k}=\boldsymbol{P}_{x, k / k-1}-\boldsymbol{K}_{k} \boldsymbol{P}_{y, k / k-1} \boldsymbol{K}_{k}^{\mathrm{T}}} \end{array}\right. \tag{2.3.3}\label{2.3.3} \]

其中:

\[ \begin{aligned} \hat{\boldsymbol{x}}_{k/k-1} &= \boldsymbol{\phi}_{k/k-1}\hat{\boldsymbol{x}}_{k-1} \\ \hat{\boldsymbol{y}}_{k/k-1} &= \boldsymbol{H}_k\hat{\boldsymbol{x}}_{k/k-1} \\ \boldsymbol{P}_{x,k/k-1} &= \boldsymbol{\phi}_{k/k-1}\boldsymbol{P}_{x,k-1}\boldsymbol{\phi}_{k/k-1}^T + \boldsymbol{B}_{k-1} \boldsymbol{Q}_{k-1} \boldsymbol{B}_{k-1}^T \\ \boldsymbol{P}_{y,k/k-1} &= \boldsymbol{H}_k \boldsymbol{P}_{x,k/k-1} \boldsymbol{H}_k^T + \boldsymbol{R}_k \\ \boldsymbol{P}_{xy,k/k-1} &= \boldsymbol{P}_{x,k/k-1} \boldsymbol{H}_k^T \end{aligned} \tag{2.3.4}\label{2.3.4} \]

Extended Kalman Filter

Error State Kalman Filter

Demo

以上内容完全参考 <惯性仪器测试与数据分析--严恭敏>,附上严重程序作为demo。


Reference

[1] <惯性仪器测试与数据分析--严恭敏> [2] http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/ [3] https://github.com/mherb/kalman [4] https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python [5] https://zhuanlan.zhihu.com/p/345927993?utm_source=wechat_session&utm_medium=social&utm_oi=722338189250936832&utm_campaign=shareopn [6] http://www.xinliang-zhong.vip/msckf_notes/ [7] https://matthewhampsey.github.io/blog/2020/07/18/mekf