最小二乘估计与卡尔曼滤波公式推导

本文是《optimal state estimation》by Dan Simon的学习笔记

相关数学背景知识

• $\frac{\partial X^TAX}{\partial X}=X^TA^T+X^TA$
• $\frac{\partial tr(ABA^T)}{\partial A}=2AB$ ($B$是对称阵)

最小二乘估计

对常值的估计

问题背静：假设需要测量一个电阻的电阻值，我们可以利用万用表测量一组测量值，但是由于测量噪声的存在，测量值和真值之间存在差别，就需要一种方法充分利用测量值得到最近接真值的估计值。
1. 不失一般性，设测量值$y=[y_1,y_2...y_k]^T$，$x=[x_1,x_2...x_n]^T$是$n$维常值向量，向量$y$是$x$的线性组合，$\hat{x}$是常量$x$的估计值。
测量值$y$和状态$x$之间存在关系：

$$\begin{equation} y_1=H_{11}+...+H_{1n}x_n+v_1 \\.\\.\\.\\ y_k=H_{1k}+...+H_{kn}x_n+v_k \end{equation}$$
写成矩阵形式为：
$$\begin{equation} y=Hx+v \end{equation}$$
其中常值矩阵$H$称为观测矩阵，其参数由系统结构决定，它描述了状态量$x$和测量值$y$的关系。矩阵$H$称为观测矩阵，其参数由系统结构决定，对于线性系统它为常值。它描述了状态量$x$和测量值$y$的关系，或者说对应于一定状态$x$，根据系统结构应该得到什么样的测量值$y$。
比如对于激光测距，位置和速度组成的状态量$x=(\begin{matrix} x\\\dot{x} \end{matrix})$和测量值$y$的关系可以描述为： $$\begin{equation} y=(\begin{matrix} 1&0\\0&0 \end{matrix})(\begin{matrix} x\\\dot{x} \end{matrix})+v\end{equation}$$
2. $y$为测量值，根据上式$H\hat{x}$可以理解为根据估计值$\hat{x}预测的测量值。$定义残差$\epsilon$为：
$$\begin{equation} \epsilon=y-H\hat{x} \end{equation}$$
它描述了测量值和预测值之间的差。差值越小则$\hat{x}$越接近真值$x$。定义代价函数$J$
$$\begin{equation} J=\epsilon^2_{y1}+...+\epsilon^2_{yk}\\=\epsilon^T_y\epsilon_y \\=(y-H\hat{x})^T(y-H\hat{x}) \end{equation}$$
3. 可以证明$J$是半正定矩阵，则$J$对$\hat{x}$求偏导并置零得到极值点即$x$的最小二乘估计值：
$$\begin{equation} \hat{x}=（H^TH）^{-1}H^Ty \end{equation}$$
其中$（H^TH）^{-1}$可逆的条件是$H满秩且k\ge n$。即测量值个数多于要估计的状态量维数。且每个测量值之间线性无关。

加权最小二乘估计

$\renewcommand\thesection{\arabic {section}}$
考虑到每次测量值的可信度不同，比如每次测量用的测量仪器存在优劣之分。我们希望对使用更可靠仪器得到的测量值在估计中给予更高的可信度，设测量值$y=Hx+v$的噪声是零均值和相互独立的。噪声的协方差矩阵

$$\begin{equation} R=E(vv^T)\\= \lgroup \begin{matrix} \sigma ^2_1&\cdots&0\\\vdots&&\vdots\\0&\cdots&\sigma ^2_k \end{matrix} \rgroup \end{equation}$$
考虑测量值可信度的代价函数：
$$\begin{equation} J=\epsilon ^2_{y1}/ \sigma^2_1 \cdots+\epsilon^2_{yk}/\sigma^2_k \\=\epsilon^T_yR^{-1}\epsilon_y\\=（y-H\hat{x})^TR^{-1}(y-H\hat{x}) \end{equation}$$
求$J$对$\hat{x}$偏导并置零,得到$x$的加权最小二乘估计：
$$\begin{equation} \hat{x}=(H^TR^{-1}H)^{-1}H^TR^{-1}y \end{equation}$$

迭代最小二乘估计

随着时间增加，我们可以得到更多的测量值$y$，求得更加精确的估计值，但测量值的增加使得向量$y$的维数$k$增加，观测矩阵$H_{k\times n}$的维度也增加。使得计算量随着时间增加。为了保持计算代价的恒定，可以采用迭代的方法进行计算。即需要解决的问题是假设利用$(k-1)$次测量得到了估计值$\hat{x}$,在得到新的测量值$y_k$,如何借助已知估计进行估计值的更新？
1. 线性迭代估计器可以写成以下形式：

$$\begin{equation} y_k=H_Kx+v_k \\\hat{x}_k=\hat{x}_{k-1}+K_k(y_k-H_k\hat{x}_{k-1}) \end{equation}$$
$(y_k-H_k\hat{x}_{k-1})$被称为校准项。公式实现了利用已知估计$\hat{x}_{k-1}$和新的测量值$y_k$得到新的估计$\hat{x}_{k}$。
2.估计器是和$K_k$无关的无偏估计，证明：
$$\begin{equation} E(\epsilon_x,k)=E(x-x^2_k)\\=(I-K_kH_k)E(\epsilon_x,k-1)-K_kE(v_k)\\=\prod^k_{i=1}E(\epsilon_x,i-1)-K_kE(v_k) \end{equation}$$
在初始估计值设置为$\hat{x}_0=E(x)$,且$E(\epsilon_x,0)=0$.得到$E(\epsilon_x,k)=E(x-\hat{x})=0$。即估计器（10）为无偏估计。且和$K_k$无关。这一条性质是我们想要的估计结论，以为我们希望估计值应该可以无限接近真值。
3.迭代参数的求解
目标函数：
$$\begin{equation} J_k=E[(x_1-\hat(x)_1)^2+(x_1-\hat(x)_1)^2+\cdots+(x_k-\hat(x)_k)^2]\\=E(tr(\epsilon_{x,k},\epsilon^2_{x,k}))=tr(P_k) \end{equation}$$
其中$P_k$称为估计误差的协方差矩阵。结合公式（11）（12）得到
$$\begin{equation} P_k=E(\epsilon_{x,k},\epsilon^2_{x,k})=E([(I-K_kH_k)\epsilon_{xk-1}-K_kv_k][\cdots]^T) \end{equation}$$
$t-1$时刻的估计误差$\epsilon_{x,k-1}$和$t$时刻的测量噪声$v_k$相互独立，因此$E(v_k \epsilon^T_{x,k-1}=E(v_k)E(\epsilon_{x,k-1})=0$
得到：
$$\begin{equation} P_k=E(\epsilon_{x,k},\epsilon^2_{x,k})=E([(I-K_kH_k)\epsilon_{x,k-1}-K_kv_k][\cdots]^T)\\=(I-K_kH_k)P_{k-1}(I-K_kH_k)^T+K_kR_kK^T_k \end{equation}$$
目标函数$J_k$对$K_k$求偏导(用到了背景知识中的公式和复合函数求偏导)：
$$\begin{equation} \frac{\partial J_k}{\partial K_k}=(I-K_kH_k)P_{k-1}(-H_k^T)+2K_kR_k \end{equation}$$
令其为0得到最小化$J_k$的$K_k$值为：
$$\begin{equation} K_k=P_{k-1}H^T_k(H_kP_{k-1}H^T_k+R_k)^{-1} \end{equation}$$
4. 参数的其他形式
通过以上求解的$K_k$和$P_k$，利用回带可以得到两个参数的其他表示形式，这里不再详细写出，具体参考下文卡尔曼方程的公式。
5. 迭代过程可以描述为已知$k-1$估计值$\hat{x}_{k-1}$，和当前时刻测量值$y_k$，更新$x_k,P_k，K_k$的过程。

迭代最小二乘估计算法流程

1. 初始化估计器
   $$\begin{equation} \hat{x}_0=E(X)\\p_0=E[(x-\hat{x}_0)([(x-\hat{x}_0)_T] \end{equation}$$
   对初始值的先验知识越多$P_0$越小。比如根据实际情况对初始值进行猜测赋值。
   2.$k=1,2,\cdots,$进行更新迭代
   （a）获取测量值$y_k$,且测量值和状态变量的关系由测量模型给出
   $$\begin{equation} y_k=H_Kx+v_k \end{equation}$$
   $v_k$ ~$N(0,R_k)$,每次测量噪声相互独立
   （b）进行估计值和参数更新
   $$\begin{equation} K_k=P_{k-1}H^T_k(H_kP_{k-1}H^T_k+R_k)^{-1}\\ \hat{x}_k=\hat{x}_{k-1}+K_k(y_k-H_k\hat{x}_{k-1})\\ P_k=(I-K_kH_k)P_{k-1}(I-K_kH_k)^T+K_kR_kK^T_k \end{equation}$$

离散卡尔曼滤波方程

一个线性系统可以由下列方式描述：

$$\begin{equation} x_k=F_{k-1}x_{k-1}+G_{k-1}u_{k-1}+w_{k-1}\\ y_k=H_kx_k+v_k \end{equation}$$
其中不同时刻的噪声项相互独立。$w_k$为系统噪声，$v_k$为测量噪声
$w_k$~$（0，Q_k）$
$v_k$~$（0，R_k）$
1. 先验估计和后验估计
已知之前所有时刻和当前时刻的测量值的估计为后验估计，记做：
$$\begin{equation} \hat{x}^+_k=E[x_k|y_1,y_2,\cdots,y_k] \end{equation}$$
已知之前k-1时刻测量值，但不知当前时刻测量值的估计为先验估计，记做:
$$\begin{equation} \hat{x}^-_k=E[x_k|y_1,y_2,\cdots,y_{k-1}] \end{equation}$$
2. 状态和方差的传播$(propagation)$
对于（）给出的线性系统，其状态$x$的均值
$$\begin{equation} \bar{x}=E(x_k)\\=F_{k-1}\bar{x}_{k-1}+G_{k-1}u_{k-1} \end{equation}$$
根据$（x_{k-1}-\bar{x}_{k-1}）$和$w_{k-1}$无关和方差定义，得到状态的方差
$$\begin{equation} P_k=E[(x_{k}-\bar{x}_{k})(\cdots)^T]\\=F_{k-1}P_{k-1}F^T_{k-1}+Q_{k-1} \end{equation}$$
由此得带均值和方差的迭代求解。且$x_k$~$(\bar{x}_k,P_k)$
3.初始化：
$$\begin{equation} \hat{x}^+_0=E(x_0)\\ P^+_0=E[(x_0-\hat{x}^+_0)(x_0-\hat{x}^+_0)^T] \end{equation}$$
4. 离散卡尔曼滤波迭代公式
预测（当前测量值未知的先验估计）：
$$\begin{equation} （1）P^-_k=F_{k-1}P^+_{k-1}F^T_{k-1}+Q_{k-1}\\ （2）\hat{x}^-_k=F_{k-1}\hat{x}^+_{k-1}+G_{k-1}u_{k-1} \end{equation}$$
校准（已知当前测量值的后验估计）：
$$\begin{equation} （3）K_k=P^-_{k-1}H^T_k(H_kP^-_{k}H^T_k+R_k)^{-1}\\=P^+_kH^T_kR^{-1}_k\\ （4）\hat{x}=\hat{x}^-+K_k(y_k-H_k\hat{x}^-_k)\\ （5）P^+_k=(I-K_kH_k)P^-_{k}(I-K_kH_k)^T+K_kR_kK^T_k\\=[(P^-_k)^{-1}+H^T_kR^{-1}_kH_k]^{-1}\\=(I-K_kH_k)P^-_k \end{equation}$$
5.性质
（a）公式（5）$P^+_k$的第一个表述比第三个表述计算更稳定，第一个表述可以保证$P^+_k$始终是对称正定的，只要$P^-_k$是对称正定的，第三个不能但计算简单，第二个表述不常用，只做推导中用。
（b）如果$K_k$使用第二个表述，则$P^+_k$需要使用第二个表述，因为两者不能同时依赖。
（c）如果${w_k}$和${v_k}$服从高斯分布，则卡尔曼滤波器是针对该问题的最优滤波器。如果两者是均值为0，无关的白噪声，但不服从高斯分布，则卡尔曼滤波器是其估计的最优线性滤波器（一个非线性滤波器可能可以找到更优的解）。
(d)更新方程：$\hat{x}=\hat{x}^-+K_k(y_k-H_k\hat{x}^-_k)$中的$y_k-H_k\hat{x}^-_k$被称为innovation term，包含了状态新的信息。