卡尔曼滤波公式推导

mean square & kalman filter 最小二乘法到卡尔曼滤波

最小二乘法是解决线性、非线性拟合问题的一般方法, 基于最朴素的假设(无偏估计、最小化误差)推导得到, 通常表现为代价函数的形式, 即统计样本的误差平方和(或均).

对于一个估计问题, 由于噪声的存在, 我们希望得到一个良好的估计, 满足: 无偏估计(均值期望一致)、均方差函数最小.

假设引入

假设一个线性时不变系统模型:
$$x(k+1)=Ax(k)+Bu(k)\text{\hspace{1em}(1)}$$
$$y(k)=Hx(k)\text{\hspace{1em}(2)}$$

$x(k)\in R^n$ 是系统k时刻的状态向量, $u(k)\in R^m$是系统的输入向量, $y(k)\in R^l$是系统的输出向量. 方程(1)称为状态方程, 是系统内部状态的递推公式, 方程(2)称为测量方程, 是外界对系统状态向量的测量结果.
对系统k时刻状态的估计用$\hat{x}(k)$表示, 误差$e(k)=x(k)-\hat{x}(k)$

观测器示例

现在假设系统的初始状态x(0)未知, 为了获取k时刻的系统状态向量信息, 假定$\hat{x}(0)$, 我们可以使用如下的观测器:
$$\hat{x}(k+1)=A\hat{x}(k)+Bu(k)\text{\hspace{1em}(3)}$$
或
$$\hat{x}(k+1)=A\hat{x}(k)+Bu(k)+K(y(k)-H\hat{x}(k))\text{\hspace{1em}(4)}$$
公式(4)称渐近观测器.
观测器(3)的状态误差为: $e_1(k+1)=A{e}_1(k)$
观测器(4)的状态误差为: $e_2(k+1)=(A-KH)e_2(k)$
上述观测器的观测性能取决于A、(A-KH)是否是渐近稳定的.
而对于任意LTS系统, 如果(A, H)是可观测的, 那么满足渐近稳定的K是一定存在的. (Kailath,1980)

卡尔曼滤波器的意义

当系统可观测且测量没有噪声时, 渐近观测器是一个较好的观测器, 现在考虑系统处理噪声w(k)与测量噪声v(k):
$$x(k+1)=Ax(k)+Bu(k)+w(k)\text{\hspace{1em}(5)}$$
$$y(k)=Hx(k)+v(k)\text{\hspace{1em}(6)}$$
为了方便推导, 一般假设w(k)、v(k)是零均值的高斯噪声, 且w(k)与v(k)与系统状态向量相互独立.
即:
$$\begin{gathered} E[w(k)]=E[v(k)]=0 \\ E[w(k)w(k{)}^T]=Q \\ E[v(k)v(k{)}^T]=R \\ E[w(k)v(k{)}^T]=0 \end{gathered}$$
由于噪声的存在, 随着时间的累计, 观测器的误差有可能越来越大, 不确定度增大, 为此基于噪声存在的高性能滤波器----卡尔曼滤波器应运而生.

定义说明

$\hat{x}(k|k-1)$: 由系统的状态方程和k-1时刻估计量预测得到,称预测项.
$\hat{x}(k|k)$: 由观测器滤波后得到,称估计项.
$\hat{x}(k|k-1)=A\hat{x}(k-1|k-1)+Bu(k-1)$
$\hat{x}(k|k)=\hat{x}(k|k-1)+K(y(k)-H\hat{x}(k|k-1))$ , 其中K称为卡尔曼增益.

$e(k|k-1)=x(k)-\hat{x}(k|k-1)$称为先验误差
$e(k|k)=x(k)-\hat{x}(k|k)$称为后验误差

对于先验估计, 有: $E[x(k)]=E[\hat{x}(k|k-1)]$
$P(k|k-1)=cov(e(k|k-1))=E[e(k|k-1)e(k|k-1{)}^T]$称为先验估计误差协方差
$P(k|k)=cov(e(k|k))=E[e(k|k)e(k|k{)}^T]$称为后验估计误差协方差

给定u(k)、y(k)和$\hat{x}(k|k-1)$下确定x(k)的线性估计.
我们希望状态估计的无偏性首要的: $E[\hat{x}(k|k)]=E[x(k)]$
同时具有最小均方误差: $mintr(P(k|k))$ (均方误差和是对应估计误差协方差矩阵的对角线之和, 即矩阵的迹)

补充一下与迹有关的矩阵求导公式:
$$tr(A^T)=tr(A)$$
$$\frac{\partial tr(AB)}{A}=B^T,\frac{\partial tr(BA)}{A}=B^T$$
$$\frac{\partial tr(AB{A}^T)}{A}=A{B}^T+AB=(如果B为对称阵)2AB$$

由于卡尔曼滤波器是递推滤波器, 实际运行时由"预测->更新->预测->更新"的不断循环, 所以我们需要解决的问题有几个:

• ① 最佳卡尔曼增益K是多少
• ② P(k|k-1)、P(k|k)如何更新

首先考察后验误差$e(k|k)$ :

$$\begin{array}{rl}e(k|k)& =x(k)-\hat{x}(k|k)\\ & =x(k)-\hat{x}(k|k-1)-K(y(k)-H\hat{x}(k|k-1))\\ & =x(k)-\hat{x}(k|k-1)-K(Hx(k)+v(k)-H\hat{x}(k|k-1))\\ & =(I-KH)[x(k)-\hat{x}(k|k-1)]-Kv(k)\end{array}\text{\hspace{1em}(6)}$$
从而, 可以得到后验误差协方差:
$$\begin{array}{rl}P(k|k)& =E(e(k|k)e(k|k{)}^T)\\ & =(I-KH)E[[x(k)-\hat{x}(k|k-1)][x(k)-\hat{x}(k|k-1){]}^T](I-KH{)}^T+KE[v(k)v(k{)}^T]K^T\\ & =(I-KH)P(k|k-1)(I-KH{)}^T+KR{K}^T\\ & =P(k|k-1)-KHP(k|k-1)-P(k|k-1)H^T{K}^T+K[HP(k|k-1)H^T+R]K^T\end{array}\text{\hspace{1em}(7)}$$

由最小化P(k|k)的迹(前面提到, 后验误差协方差的迹为均方误差和), 求导:
$$\frac{\partial (P(k|k))}{K}=-2P(k|k-1)H^T+2K[HP(k|k-1)H^T+R]$$
令偏导为0, 得到:
$$K=P(k|k-1)H^T\ast [HP(k|k-1)H^T+R{]}^{-1}\text{\hspace{1em}(8)}$$
公式(8)称为最佳卡尔曼增益公式.

代入公式(7), 得到P(k|k)的更新公式:
$$P(k|k)=(I-KH)P(k|k-1)\text{\hspace{1em}(9)}$$

还缺一个P(k|k-1)的更新公式, 考察先验误差e(k+1|k):
$$\begin{array}{rl}e(k+1|k)& =x(k+1)-\hat{x}(k+1|k)\\ & =Ax(k)+Bu(k)+w(k)-(A\hat{x}(k|k)+Bu(k))\\ & =A[x(k)-\hat{x}(k|k)]+w(k)\\ & =Ae(k|k)+w(k)\end{array}\text{\hspace{1em}(10)}$$

$$\begin{array}{rl}P(k|k-1)& =E[e(k|k-1)e(k|k-1{)}^T]\\ & =E[(Ae(k-1|k-1)+w(k-1))(Ae(k-1|k-1)+w(k-1){)}^T]\\ & =E[Ae(k-1|k-1)e(k-1|k-1{)}^T{A}^T+Ae(k-1|k-1)w(k-1{)}^T+w(k-1)e(k-1|k-1{)}^T{A}^T+w(k-1)w(k-1{)}^T]\\ & =AE[e(k-1|k-1)e(k-1|k-1{)}^T]A^T+E[w(k-1)w(k-1{)}^T]\\ & =AP(k-1|k-1)A^T+Q\end{array}\text{\hspace{1em}(11)}$$

至此,我们解决了卡尔曼滤波器依赖的三个黄金公式: 公式(8)、(9)、(11).

一个规范的卡尔曼过程如下:

• 给定初始状态估计 $\hat{x}(0|0)$,初始误差协方差$P(0|0)$ 系统处理噪声方差Q, 测量噪声方差R
• 估计k时刻的状态: $\hat{x}(k|k-1)=A\hat{x}(k-1|k-1)+Bu(k)$
• 更新先验误差协方差: $P(k|k-1)=AP(k-1|k-1)A^T+Q$
• 计算最佳卡尔曼增益: $K=P(k|k-1)H^T[HP(k|k-1)H^T+R{]}^{-1}$
• 更新后验误差协方差: $P(k|k)=(I-KH)P(k|k-1)$
• 计算状态向量的误差项: $\hat{e}(k)=y(k)-Hx(k|k-1)$
• 计算状态向量的后验估计: $\hat{x}(k|k)=\hat{x}(k|k-1)+K\ast \hat{e}(k)$
• 计算系统输出的后验估计: $\hat{y}(k)=H\hat{x}(k|k)$