深入理解卡尔曼滤波

1. 最小二乘（LS）、加权最小二乘估计（WLS）、递推最小二乘（RLS）

观测方程
![](http://latex.codecogs.com/gif.latex?
y = Hx + v
)
测量残差
![](http://latex.codecogs.com/gif.latex?
\varepsilon = y - H\hat{x}
)
代价函数
![](http://latex.codecogs.com/gif.latex? \begin{align}
\varepsilon &= (y-H\hat{x})^T (y-H\hat{x})\
& = y^Ty - \hat{x}^THTy - y^{TH\hat{x}+\hat{x}}TH^TH\hat{x}
\end{align}
)
最优准则
![](http://latex.codecogs.com/gif.latex? \begin{align}
\frac{\partial {J}}{\partial {\hat{x}}} = - y^TH - y^TH + 2\hat{x}^THTH = 0
\end{align}
)
求解这个方程，得
![](http://latex.codecogs.com/gif.latex?
\begin{align}
Hy^T &= H^TH\hat{x}\
x &= (H^TH){-1}H^Ty = H^Ly
\end{align}
)
其中，H^L是H的左伪逆矩阵

2. 最小均方误差估计（MSE）

3. 卡尔曼滤波

3.1 理解

• 基于最小均方误差准则MMSE
• 线性系统
• 递推算法
• 最优滤波算法

卡尔曼滤波器稳态误差和稳态误差的方差？

3.2 使用过程中注意的问题

• 滤波器系数的确定
  由系统模型、观测模型、采样周期来确定

状态方程
 = A(t_k,t_{k-1}) x(t_{k-1})+ B(t_{k-1})u(t_{k-1}) + w(t_{k-1}))

观测方程
=C(t_k)x(t_k)+ v(t_k))

• 滤波器初值的确定

• 过程噪声和测量噪声的方差估计

卡尔曼滤波器设计和调试中重要而困难的一步
噪声建模：　自相关、功率谱密度、阿伦方差
假设检验： 构造统计量，做统计实验，对总体分布进行假设检验
（1） 仪器仪表说明书，一般有厂家标定的功率谱度量值
（2） 采集实验数据做自相关（功率谱）分析、艾伦方差分析

3.3 测量噪声和误差源如何从物理意义上理解过程噪声和观测噪声？

过程噪声：激励噪声，非可控激励（输入）。激励是改变状态的原因，从改变状态的原因里扣除可控的部分之后剩下的部分即为激励噪声。
观测噪声：由测量误差源激励的测量系统的输出。

《现代控制理论与应用》 齐晓慧 国防工业出版社

3.4 测量噪声和误差源

以GPS定位系统为例，伪距测量噪声由所有误差源（卫星钟差、星历误差、电离层延时、对流程延时、 多径、接收机噪声（热噪声、量化噪声等）、接收机钟差）折合成测距误差构成。

一般来说，测量误差源包括：

• 噪声(noise)
• 偏差(offset)
• 比例因子（scale）
• 非线性（Nonlinearity）
• 非正交（Nonorthogonal）【矢量传感器】
• 非对准（Misalignment）【矢量传感器】
• 死区误差
• 量化误差

4. 案例——卡尔曼滤波用于导航定位

状态变量：
![](http://latex.codecogs.com/gif.latex?
\textbf{ x }= [ x,y,z,v_x,v_y,v_z,\delta{t_u},\delta{f_u}]^T )
状态模型
接收机时钟模型
![](http://latex.codecogs.com/gif.latex?
\begin{align}
\dot{\delta{t_u} } &= \delta{f_u} + e_t \
\dot{\delta{f_u} } &= e_f
\end{align}
)
用户运动模型
![](http://latex.codecogs.com/gif.latex?
\begin{align}
\dot{v_x} = \dot{x} + e_{ax} \
\dot{v_y} = \dot{y} + e_{ay} \
\dot{v_z} = \dot{z} + e_{az}
\end{align}
)
观测模型
伪距观测量
![](http://latex.codecogs.com/gif.latex?
\begin{align}
\rho^{n} = r^{n} + \delta{t_u} - \delta{t_s^n} + I^n + T^n + \varepsilon_p^{n}
\end{align}
)
伪距观测值
![](http://latex.codecogs.com/gif.latex?
\begin{align}
\rho^{n} = c \cdot (t_u - t_s^n)
\end{align}
)

伪距率观测量
![](http://latex.codecogs.com/gif.latex?
\begin{align}
\dot\rho^{n} &= \dot r^{n} + \delta{f_u} - \delta{f_s^n} + \varepsilon_{\dot{p}}^{n} \
\dot r^n &= (v_s^n - v) \cdot \emph{1}^n
\end{align}
)
整理，得
![](http://latex.codecogs.com/gif.latex?
\begin{align}
-v\cdot\emph{1}^n + \cdot{f_u} =( \dot{\rho}^n - v_s^n \cdot \dot{\emph{1}^n} + \delta{f_s^n}) - \varepsilon_{\dot p}^n
\end{align}
)
伪距率观测值
![](http://latex.codecogs.com/gif.latex?
\begin{align}
\dot\rho = -\lambda f_d^n = -\lambda(\phi_{k}^n - \phi_{k-1}^n )
\end{align}
)