Kalman Filter

这是上学期《最优估计》课上的一部分内容，通过网上的内容以及自己的知识进行整理。
关于各种通俗易懂的卡尔曼滤波我应该讲不到【参考1】那么好了，所以尽量往深度写，8月14日写完发知乎加油(ง •_•)ง

简单介绍

卡尔曼滤波的故事

卡尔曼滤波的名称来自于鲁道夫·卡尔曼（1930-2016），卡尔曼滤波是最优估计算法(optimal estimation algorithm)算法的一种。以人名命名的滤波算法还有维纳滤波(Wiener Filter)。小心假设在【参考2】下的回答引用了【参考9】

其实维纳滤波更好理解一些为什么叫滤波，因为它剔除了信号中的特定频率的信号。而其它则不那么直观，但本质上是使得数据更加真实了。

以原理命名的滤波有、中位值滤波、算术平均滤波、递推平均滤波、中位值平均滤波、加权递推平均滤波、一阶滞后滤波、限幅滤波、限幅平均滤波、消抖滤波、限幅消抖滤波、IR数字滤波器等。

通过卡尔曼滤波衍生出的滤波还有扩展的卡尔曼滤波(Extend Kalman Filter)、平方根滤波(Square Root Filter)、信息滤波(Information Filter)、平方根信息滤波(Square Root Information Filter)、UD分解滤波、UDU分解滤波等。后面文章都用KF替代了，因为原理上都是类似的，如果计算没有问题结果应是相同的(现实当然不)。

还有一类滤波叫做粒子滤波(Particle Filter)，这我就更不了解了。

KF其实和最小二乘估计有点像，如果把状态方程的一步预测作为一个“虚拟观测值”，那么处理起来和最小二乘是一样的。

KF的使用范围？

好像是高斯马尔可夫过程才可。

KF的使用

在导航方面有很多应用，在导航方面可以这么理解

观测一个人的运动，前一时刻距离你10m，速度是1m/s向你走过来，精度分别是1m和0.1m/s，并且你假设他是匀速直线运动；
1s后你可以估计他距离你9m，速度1m/s，这叫一步预测，同时可以算出它的精度。
其实你也可以通过其它方面得到它的位置，比如你观测到了他距离你9.1m，精度1m。
这时候怎么办呢？当然是结合起来计算，这就是卡尔曼滤波。

KF的使用

（ 1 ）星载GPS实时定轨

论文[1]中研制星载GPS实时定轨软件SATODS，简单介绍其中使用的原理。

状态方程使用动力学模型补偿算法(DMC)，用一阶高斯-马尔可夫模型来模拟没有模型化的卫星加速度即式（1-1），KF递推估计得到式（1-2）；将状态向量扩展得到式（1-3），最终实时定轨滤波模型的状态方程为式（1-4），各参数见式（1-5）；观测方程为式（1-6）

\[\dot{w}(t)=-\beta w(t)+u(t)\tag{1-1} \]

\[w(t)=w(t_0)e^{-\beta(t-t_0)}+\int_{t_0}^t e^{-\beta(t-\tau)}u(\tau)\text{d}\tau\tag{1-2} \]

\[X=\left[\begin{matrix}r&v&w&\beta&C_d&C_r&\delta_t\end{matrix}\right]^T\tag{1-3} \]

\[X(t)=\Phi(t,t_0)X(t_0)+\int_{t_0}^t\Phi(t,\tau)G(\tau)U(\tau)\text{d}\tau\tag{1-4} \]

\[G(t)=\left[\begin{matrix} 0_{3\times 3} &0_{3\times 3}\\ 0_{3\times 3} &0_{3\times 3}\\ I_{3\times 3} &0_{3\times 3}\\ 0_{3\times 3} &I_{3\times 3} \end{matrix}\right], U(t)=\left[\begin{matrix} u(t)\\ u_\beta(t)\\ u_{C_d}(t)\\ u_{C_r}(t) \end{matrix}\right]\tag{1-5}\]

\[P^i=\rho^i-c\cdot \delta_t^i+c\cdot \delta_t+\delta_{\rho_{iono}}+\varepsilon_P\tag{1-6} \]

\(w(t)\)为RTN坐标系下的没有模型化的三维加速度
\(\beta\)为相关事时间的倒数，随机游走模型
\(u(t)\)为零均值高斯白噪声过程
\(r\)为地心惯性系下的卫星位置
\(v\)为地心惯性系下的速度向量
\(C_d\)为大气阻力系数，随机游走模型
\(C_r\)为太阳光压系数，随机游走模型
\(\delta_t\)为观测模型中的星载接收机钟差，高斯白噪声
\(\Phi(t,t_0)\)为状态转移矩阵
\(u_\beta,u_{C_d},u_{C_r}\)均为零值高斯白噪声过程
\(U(t)\)为零值白噪声过程
\(P^i\)为卫星\(i\)到星载接收机间的伪距
\(\rho^i\)为信号发射时刻卫星\(i\)到星载接收机间的几何距离
\(\delta_t^i\)为卫星钟差
\(\delta_t\)为星载接收机钟差
\(\delta_{\rho_{iono}}\)为电离层延迟
\(\varepsilon_P\)为伪距观测噪声
\(c\)为真空中的光速

（ 2 ）星载GPS卫星自主定轨

论文[2]中采用简化动力学推广卡尔曼滤波，中间使用了Grubbs准则（与本文无关）。卫星状态向量为（2-1）式，（2-2）为状态方程，（2-3）为时间更新，（2-4）为测量更新，（2-5）为观测方程（频伪距的无电离层影响组合作）

\[X=\left[\begin{matrix}r&\dot{r}\end{matrix}\right]^T\tag{2-1} \]

\[\dot{\tilde{X}}=F(\tilde{X},t_k),\tilde{X}(t_{k-1})=\hat{X}_{k-1}, \dot{\widetilde{\Phi}}(t_k,t_{k-1})=A(t_k)\widetilde{\Phi}(t_k,t_{k-1})\tag{2-2}\]

\[\tilde{P}_k=\tilde{\Phi}(t_k,t_{k-1})\hat{P}_{k-1}\tilde{\Phi}^T(t_k,t_{k-1})+\hat{Q}_k\tag{2-3} \]

\[K_k=\tilde{P}_kH_k^T\left[H_k\tilde{P}_kH_k^T+R_k\right]^{-1}\\ \hat{X}_k=\tilde{X}_k+K_k(Y_k-\tilde{X}_k)\\ \hat{P}_k=(I-K_kH_k)\tilde{P}_k,H_k=\left[\begin{matrix}I_{3\times 3}&0_{3\times 3}\end{matrix}\right]\tag{2-4}\]

\[P_C(t)=\rho(t)+c\delta_{t_{Leo}}(t)-c\delta_t(t)+v_{P_C}\tag{2-5} \]

\(r\)是卫星在某一时元的三维位置
\(\dot{r}\)是卫星在某一时元的速度向量
\(X_k\)为\(k\)时元卫星状态
\(\hat{X}_{k-1}\)为\(k-1\)时元的先验状态
\(\hat{P}_{k-1}\)为\(k-1\)时元状态的协方差矩阵
\(\hat{Q}_{k-1}\)为\(k-1\)时元模型误差协防擦矩阵
\(Y_k\)为\(k\)时元的观测值，在此为GPS绝对定位计算结果，即卫星的三维位置和速度
\(R_k\)为\(Y_k\)对应的协方差矩阵
\(P_C(t)\)为\(t\)时元双频伪距的组合观测值
\(\rho(t)\)为\(t\)时元的GPS卫星发射天线知卫星星载GPS接受天线的几何距离
\(\delta_{t_{Leo}}(t)\)为\(t\)时元星载GPS接收机的钟差（待求量）
\(\delta_t(t)\)为\(t\)时元GPS卫星的钟差，利用导航电文钟差参数直接计算
\(v_{P_C}\)为组合观测值的测量噪声

（ 3 ）基于车轮安装惯性测量单元的车载组合导航

论文[3]和[4]大致介绍了同一内容，[4]时学位论文比[3]更加完善。

（ 4 ）GPS/INS组合系统数据处理方法

下面引用自论文[5]，总的来说就是松组合简单但精度低，紧组合复杂但精度高。文章研究了伪距双差GPS/INS组合系统，辅以相位平滑伪距差分，同时施用模拟的多路径效应和多普勒改正，采用联邦Kalman滤波进行数据处理。

GPS和INS的组合可以有两种形式。一种为松组合模式（Loose Coupling），另一种是紧组合模式（Tight Coupling）。
松组合的工作原理是：利用GPS重调INS的输出，即用GPS输出的位置和速度信息直接调整INS的漂移误差，得到精确的位置、速度和姿态信息。当GPS正常工作时，系统输出为GPS信息；而当GPS中断时，INS以GPS停止工作的瞬时值为初始值继续工作，系统输出为INS信息，直到下一个GPS工作历元出现为止。
松组合的优点包括：GPS和INS保持了各自的独立性，其中任何一个出现故障时，系统仍能继续工作；组合导航系统结构简单，便于设计；GPS接收机和INS的开发和调试独立性强，便于系统的故障检测和隔离；组合系统的开发周期短。
其缺点是：组合后GPS接收机的抗干扰能力和动态跟踪能力没有得到任何改善，组合系统的导航精度没有紧组合模式高。
紧组合工作原理是：利用INS输出的位置和速度信息来估计GPS的伪距和伪距率，且与GPS输出的伪距和伪距率进行比较，将差值构筑系统的观测方程，经Kalman滤波，可得到精确GPS和INS输出信息。
紧组合模式的优点是：GPS接收机向INS提供精确的位置和速度信息，辅助并帮助克服INS的长时间漂移误差积累；INS同时向GPS接收机提供实时的位置和速度信息，辅助GPS接收机内部的码/载波跟踪回路，提高GPS接收机的抗干扰能力和动态跟踪能力；辅助后的GPS接收机可以收到更多的卫星信息，而综合滤波器可以利用尽可能多的卫星信息提高滤波修正的精度；能够对GPS接收机的信息完整性进行监测。
缺点是：GPS与INS的硬件和软件都必须进行统筹规划、联合设计和调试，对系统时钟的同步性要求较高，组合系统的结构比较复杂。

联邦Kalman滤波方法是指对系统内的各个子系统先进行预处理，即用标准Kalman滤波预处理各个子系统的观测数据，获得输出；在此基础上，构筑整个组合系统的状态方程和观测方程，进行所谓的中央Kalman滤波处理，求得精确的位置和姿态角。

（ 5 ）附有道路信息约束的自适应卡尔曼滤波

期刊文章[6]讲了将道路信息作为一种观测信息，提高滤波精度的方法。

（ 6 ）基于RSSI抗差滤波的WiFi定位

期刊文章[7]采用抗差M估计方法进行信号强度(received signal strength indication,RSSI)的卡尔曼滤波。

WiFi的工作频段在2.4GHz，自由空间中，接受设备的接受功率由Friis传播方程（6-1）。M估计的准则是寻找一个非负的凸标量函数使得成立（6-2），对（6-2）中的\(X^k\)求导为0得（6-3），写成矩阵得到（6-4）、（6-5），根据新息来调节增益矩阵\(K_k\)可以削弱或消除粗差的影响，调整得到增益矩阵（6-6），卡尔曼滤波得递推公式为（6-7）,一般对于具有左偏特性得分布等价权函数\(D(e_k)\)为（6-8），滤波的状态方程为（6-9），观测方程为（6-10），线性化后的状态转移矩阵为（6-11）

\[P_r(d)=\frac{P_tG_tG_r\lambda^2}{(4\pi)^2d^2}\tag{6-1} \]

\[\min=\sum_{i=1}^n f(Z_i^k-h_iX^k)\tag{6-2} \]

\[\left(\begin{matrix} f'(e_1)h_{11}+f'(e_2)h_{21}+\cdots+f'(e_n)h_{n1}\\ f'(e_1)h_{12}+f'(e_2)h_{22}+\cdots+f'(e_n)h_{n2}\\ \vdots\\ f'(e_1)h_{1m}+f'(e_2)h_{2m}+\cdots+f'(e_n)h_{nm}\\ \end{matrix}\right)=0\tag{6-3}\]

\[\left(\begin{matrix} h_{11} &\cdots &h_{n1}\\ \vdots & &\vdots\\ h_{1m} &\cdots &h_{nm} \end{matrix}\right) \left(\begin{matrix} D(e_1) &0 &\cdots &0\\ 0 &D(e_2) &\cdots &0\\ \vdots &\vdots &\ddots &\vdots\\ 0 &0 &\cdots &D(e_n) \end{matrix}\right) \left(\begin{matrix} e_1\\ e_2\\ \ddots\\ e_n \end{matrix}\right)=0\tag{6-4}\]

\[H_k^TD_k(Z_k-H_kX)=0\tag{6-5} \]

\[\hat{K}_k=K_kD\tag{6-6} \]

\[\begin{cases} \hat{X}_k(-)=\Phi_{k-1}\hat{X}_{k-1}(+)\\ P_k(-)=\Phi_{k-1}P_{k-1}(+)\Phi_{k-1}^T+Q_{k-1}\\ \hat{X}_k=P_k(-1)H_k^T\left[H_kP_k(-)H_k^T+R_k\right]^{-1}D_k\\ \hat{X}_k(+)=\hat{X}_k(-)+\hat{K}_kD[Z_k-H_k\hat{X}_k(-)]\\ P_k(+)=(I-\hat{K}_kH_k)P_k(-) \end{cases}\tag{6-7}\]

\[D(e_k)=\begin{cases}0, &e_k<-C\\ -\frac{K_0}{e_k}\left(\frac{C+e_k}{C-K_0}\right),&-C\leqslant e_k<-K_0\\ 1,&-K_0<e_k<K_0\\ -\frac{K_0}{e_k}\left(\frac{C+e_k}{C-K_0}\right)^2,&K_0\leqslant |e_k|<K_1\\ 0,&e_k\geqslant K_1 \end{cases}\tag{6-8}\]

\[\begin{cases} P_r(d_{k+1})=P_r(d_k)+20\lg\left(\frac{d_k}{d_k+v_kt}\right)\\ d_{k+1}=d_k+v_kt+\frac{1}{2}at^2\\ v_{k+1}=v_k+at \end{cases}\tag{6-9}\]

\[\bar{P}_r=\left(\begin{matrix}1&0&0\end{matrix}\right) \left(\begin{matrix}P_r(d_{k+1})\\d_{k+1}\\v_{k+1}\end{matrix}\right)+r_{k+1}\tag{6-10}\]

\[\Phi=\left(\begin{matrix} 1 &\left(1+\frac{v_k^0t}{d_k^0}\right)\left(\frac{1}{d_k^0(d_k^0+v_k^0t)}-\frac{1}{(d_k^0+v_k^0t)^2}\right) &-\frac{t}{d_k^0+v_k^0t}\\ 0 &1 &t\\ 0 &0 &1 \end{matrix}\right)\tag{6-11}\]

\(P_r\)为接受功率
\(P_t\)为发射功率
\(G_t\)为发射天线增益
\(G_r\)为接收天线增益
\(\lambda\)为发射信号的波长
\(d\)为收发天线之间的距离
\(Z_i^k\)表示时刻k观测向量中第\(i\)个元素，观测向量的维数是\(n\)
\(h_i\)表示观测矩阵\(H\)的第\(i\)行，\(h_i=(h_{i1},h_{i2},h_{i3},\cdots,h_{im})\)，\(m\)是待估向量的维数
\(e(k)_i=Z_i^k-h_iX^k\)
\(D(e_i)=\frac{f'(e_i)}{e_i}\)
\(D_k=\text{diag} [D(e_1),D_(e_2),\cdots,D(e_n)]\)
\(K_k\)为增益矩阵
\(D\)为等价权函数
\(K_0,K_1，C\)可根据经验选取
\(d\)表示用户当前位置距离某\(AP\)的距离
\(t\)表示采样间隔
\(a\)表示采样间隔时间内用户的运动加速度
\(v\)表示用户当前运动速度，远离AP时为正，靠近AP时为负
\(d_k^0、v_k^0\)表示\(k\)时刻的状态概率值

（ 7 ）抗差卡尔曼滤波&动态水准网

文章[8]使用广义最小二乘推导了卡尔曼滤波，讲到了稳健估计的基本方法——稳健M估计。（7-1）式给出来状态方程和观测方程，系统的随机模型为（7-2），对误差方程分别按最小二乘准则和M估计准则为（7-3）式，M估计的关键是选择合适的\(\rho(v)\)或\(\omega_i\)，目前的两种方法为Huber法（7-4）和IGGⅢ法（7-5）

\[\begin{cases} X_{k+1}=\Phi_{k+1,k}X_k+\Gamma_{k+1,k}\Omega_k\\L_{k+1}=B_{k+1}X_{k+1}+\Delta_{k=1} \end{cases}\tag{7-1}\]

\[\begin{cases} E(\Omega_k)=0,E(\Delta_k)=0\\ \mathbb{cov}(\Omega_k,\Omega_j)=D_\Omega(k)\delta_{kj}\\ \mathbb{cov}(\Delta_k,\Delta_j)=D_\Delta(k)\delta_{kj}\\ \mathbb{cov}(\Delta_k,\Omega_j)=0\\ E(X_0)=\mu_X(0)=\hat{X}(0/0)\\ \mathbb{var}(X_0)=D_X(0)\\ \mathbb{cov}(X_0,\Omega_k)=0\\ \mathbb{cov}(X_0,\Delta_k)=0 \end{cases}\tag{7-2}\]

\[\begin{cases} V=A\hat{X}-L\\ \sum P_iv_i^2=\min&\color{red}{\Rightarrow} \hat{X}=(A^TPA)^{-1}A^TPL\\ \sum \rho(v_i)=\min &\color{red}{\Rightarrow} \varphi(v_i)=\rho'(v_i),\sum \varphi(v_i)a_i=0,\frac{\varphi(v_i)}{v_i}=\omega_i\color{orange}{(\omega_i为权因子)} \\&\color{red}{\Rightarrow}\sum \omega_iv_ia_i=0 \\&\color{red}{\Rightarrow} \bar{P}=\{\bar{P}_i\}=\{P_i\omega_i\}\color{orange}{(原观测值的权为P_i)} \\&\color{red}{\Rightarrow} A^t\bar{P}A=0\\ &\color{red}{\Rightarrow} \hat{X}=(A^T\bar{P}A)^{-1}A^T\bar{P}L \end{cases}\tag{7-3}\]

\[\text{Huber}法\begin{cases} \rho(v_i)=\begin{cases} v_i^2/2 &|v_i|\leqslant k\sigma_{v_i}\\ k|v_i|-k^2/2 &|v_i|>k\sigma_{v_i} \end{cases}\\ \omega_i=\begin{cases} 1 &|v_i|\leqslant k\sigma_{v_i}\\ k\sigma_0/|v_i| &|v_i|>k\sigma_{v_i} \end{cases} \end{cases}\tag{7-4}\]

\[\text{IGGⅢ法}\begin{cases} 1 &|v_i|\leqslant k_0\sigma_{v_i}\\ \frac{k_0\sigma_0}{v_i}\times \left[\frac{k_1-|v_i|/\sigma_0}{k_1-k_0}\right]^2 &k_0\sigma_{v_i}< |v_i|\leqslant k_1\sigma_{v_i}\\ 1 & |v_i|>k_1\sigma_{v_i}\\ \color{red}{k_0,k_1根据实际需要分别取1.5~2.5，3.0~5.0} \end{cases}\tag{7-5}\]

( 8 )自适应卡尔曼滤波在GPS/DR组合导航中的应用

针对联邦卡尔曼滤波模式的缺点，文章[9]采用紧组合卡尔曼滤波模式，紧组合式卡尔曼滤波可分为两种形式：GPS伪距与DR原始信息的紧组合卡尔曼滤波和基于GPS伪距、伪距率与DR原始数据的紧组合式卡尔曼滤波。状态方程为式（8-1），其中各符号含义见（8-2）-（8-5），观测方程为（8-6），若为GPS伪距与DR为（8-7），GPS伪距、伪距率与DR为（8-8），看起来好像是一样的。利用抗差估计和状态方差矩阵膨胀模型相结合的自适应滤波和标准卡尔曼滤波的区别仅在于计算状态估计协方差的过程，见式（8-9），其中\(\lambda_k\)的计算见式（8-10）

\[X_k=\Phi_{k,k-1}X_{k-1}+\Gamma_{k,k-1}w_k\tag{8-1} \]

\[X_k=\left(\begin{matrix}X&Y&Z&\delta_t&\dot{X}&\dot{Y}&\dot{Z}&\dot{\delta}_t\end{matrix}\right)^T\tag{8-2} \]

\[w_k=\color{red}{?这篇文章看不清}\tag{8-3} \]

\[\Phi_{k,k-1}=\left[\begin{matrix} 1 &0 &0 &0 &\text{d}t &0 &0 &0\\ 0 &1 &0 &0 &0 &\text{d}t &0 &0\\ 0 &0 &1 &0 &0 &0 &\text{d}t &0\\ 0 &0 &0 &1 &0 &0 &0 &\text{d}t\\ 0 &0 &0 &0 &1 &0 &0 &0\\ 0 &0 &0 &0 &0 &1 &0 &0\\ 0 &0 &0 &0 &0 &0 &1 &0\\ 0 &0 &0 &0 &0 &0 &0 &1 \end{matrix}\right]=\left[\begin{matrix} I_{4\times4} &\text{d}tI_{4\times4}\\ 0_{4\times4} &I_{4\times4} \end{matrix}\right]\tag{8-4}\]

\[\Gamma_{k,k-1}=\left[\begin{matrix} \frac{\text{d}^2t}{2} &0 &0 &0\\ 0 &\frac{\text{d}^2t}{2} &0 &0\\ 0 &0 &\frac{\text{d}^2t}{2} &0\\ 0 &0 &0 &\frac{\text{d}^2t}{2}\\ \text{d}t &0 &0 &0\\ 0 &\text{d}t &0 &0\\ 0 &0 &\text{d}t &0\\ 0 &0 &0 &\text{d}t \end{matrix}\right]=\left[\begin{matrix} \frac{\text{d}^2t}{2}I_{4\times 4}\\ \text{d}tI_{4\times 4} \end{matrix}\right]\tag{8-5}\]

\[Z_{gd}=H_{gd}X+V_{gd}\tag{8-6} \]

\[Z_{gd}=\left[\begin{matrix} \delta\rho_i\\Z_d\end{matrix}\right], H_{gd}=\left[\begin{matrix} H_{gi}\\H_d\end{matrix}\right], V_{gd}=\left[\begin{matrix} V_{gi}\\V_d\end{matrix}\right]\tag{8-7}\]

\[Z_{gd}=\left[\begin{matrix} \delta\rho_i\\Z_d\end{matrix}\right], H_{gd}=\left[\begin{matrix} H_g\\H_d\end{matrix}\right], V_{gd}=\left[\begin{matrix} V_g\\V_d\end{matrix}\right]\tag{8-8}\]

\[P_{k,k-1}=\lambda_k\Phi_{k,k-1}P_{k-1}\Phi_{k,k-1}^T+\Gamma_{k-1}Q_{k-1}\Gamma_{k-1}^T\tag{8-9} \]

\[\begin{cases} \lambda_k=\max\{1,a\}\\ a=\frac{V_k^TV_k-\mathbb{tr}(N)}{\mathbb{tr}(M)}\\ V_k=Z_k-H_kX_{k,k-1}\\ N=H_kQ_kH_k^T+R_k\\ M=H_k\Phi_{k,k-1}P_{k-1}\Phi_{k,k-1}^TH_k^T \end{cases}\tag{8-10}\]

状态向量：接收机三维方向的位置、速度以及接收机钟差、钟速
\(w_k\)为当前历元的模型误差向量
\(\Phi_{k,k-1}、\Gamma_{k,k-1}\)代表状态转移矩阵和系统噪声矩阵
\(\text{d}t\)为GPS采样间隔
原文：为了解决高动态情况下的滤波发散问题，针对不同的运动状态可构造出不同的自适应卡尔曼滤波算法，如Sage滤波、基于多模型的自适应滤波(MMAE)、利用抗差估计和状态协方差矩阵膨胀模型相结合的自适应滤波等等。
\(\lambda_k\)称为渐消因子或遗忘因子

GPS/INS松组合与紧组合的实现与定位精度比较

文章{10]在经过对比之后得到结论：

在卫星信号接受较好的时间段，紧组合与松组合在定位精度上没有明显的区别；信号接受较差的时间段，两种组合方式之间均方根误差值在0.7m左右。
并且给出理由：
松组合需要接收机多于4颗卫星信号才可以实现其组合，而紧组合不需要这一约束条件。
但是我看完之后觉得差距并不是很大。。。在信号差的时候都会到20m左右的误差。但是按照均方根来看确实还可以