前言

不知道是不是每个想入局卡尔曼的同学都面临同样的问题，敲笔记之前浏览了一遍参考的东西，各种角度给出了非常生动的介绍，手里还有一本清华大学张贤达老师《现代信号处理》也从自适应滤波器的角度对卡尔曼滤波器的推导。虽然大模型也能给出示例程序，运行结果也能感受的到算法的魅力，但肉眼凡胎还是要亲自学习理解一下，那就学院派先来，笔记一下张老师的推导思路。

从自适应滤波的角度

自适应滤波器发展的历史中，匹配滤波器和维纳滤波器是绕不开的两座丰碑，匹配滤波器是输出达到最大的信噪比，一个经典的应用就是通信系统自带training sequence或pilot信号的解调。wiener滤波器采用最小均方估计误差的准则，不需要提供先验的同步信号，应用就更加广泛了。这个经典的推导再很多年前的webrtc中的噪声抑制之一：频域维纳滤波 敲过键盘。这里有很多关于极点的扩展分析，暂时涉及不到，就不深挖这个坑了。而数字信号处理中，估计误差$e(n)$定义为期望响应$d(n)$与滤波器输出$y(n)$之差，即$$y(n)=\sum _{k=0}^{\infty }{w}_k^{\ast }u(n-k),n=1,2,...$$$$e(n)=d(n)-y(n)$$这种估计误差在某种统计意义下最小的设计就被称为最优滤波器，优化准则即令某个代价函数最小化，在ai时代的熵函数盛行之前，最小均方误差应该是最核心的存在，这就是大名鼎鼎的：MMSE(Minimum Mean Square Error)，简而言之：设计线性离散时间滤波器的系数$w_k$，使输出$y(n)$在给定的输入样本集合$u(0),u(1),...$的情况下给出期望响应$d(n)$的估计，并使得$e(n)=d(n)-y(n)$的均方误差$E|e(n){|}^2$为最小。

正交性原理到维纳解

从数学上课文还推导了正交性原理，并讲述其从数学上作为线性最有滤波理论中令代价函数$J$最小化的充分必要条件之关系。直接写出定理和引理E{u(n−k)eopt∗(n)}=0,k=0,1,2E\{u(n-k)e^*_{opt}(n)\}=0, \ \ \ k=0,1,2E{u(n−k)eopt∗​(n)}=0,   k=0,1,2E{yopt(n)eopt∗(n)}=0E\{y_{opt}(n)e^*_{opt}(n)\}=0E{yopt​(n)eopt∗​(n)}=0根据前面的推导，可以将定理展开E{u(n−k)[d∗(n)−∑i=0∞wopt,ku∗(n−i)]}=0,k=0,1,2...E\{u(n-k) [ d^*(n)-\sum_{i=0}^\infty w_{opt,k} u^*(n-i)]\}=0, \ \ \ k=0,1,2...E{u(n−k)[d∗(n)−i=0∑∞​wopt,k​u∗(n−i)]}=0,   k=0,1,2...∑i=0∞wopt,kE{u(n−k)u∗(n−i)}=E{u(n−k)d∗(n)},k=0,1,2...\sum_{i=0}^\infty w_{opt,k} E\{u(n-k) u^*(n-i)\}=E\{u(n-k) d^*(n)\}, \ \ \ k=0,1,2...i=0∑∞​wopt,k​E{u(n−k)u∗(n−i)}=E{u(n−k)d∗(n)},   k=0,1,2...式中，数学期望项E{u(n−k)u∗(n−i)}E\{u(n-k) u^*(n-i)\}E{u(n−k)u∗(n−i)}代表v滤波器输入在之后$i-k$的自相关函数$R_{u,u}(i-k)$，即Ru,u(i−k)=E{u(n−k)u∗(n−i)}R_{u,u}(i-k)=E\{u(n-k) u^*(n-i)\}Ru,u​(i−k)=E{u(n−k)u∗(n−i)}而数学期望项E{u(n−k)d∗(n)}E\{u(n-k) d^*(n)\}E{u(n−k)d∗(n)}等于v滤波器输入$u(n-k)$与数学期望$d(n)$在滞后$-k$的互相关函数$R_{u,d}(-k)$ Ru,d(−k)=E{u(n−k)d∗(n)}R_{u,d}(-k)=E\{u(n-k) d^*(n)\}Ru,d​(−k)=E{u(n−k)d∗(n)}由此得到一个更简洁的公式：$$\sum _{i=0}^{\infty }{w}_{opt,k}{R}_{u,u}(i-k)\}=R_{u,d}(-k),k=0,1,\mathrm{2...}$$这就是Wiener-Hopf 差分方程，对应M阶FIR滤波器，正无穷这个东西就变成了M-1，所有有限长FIR的M个齐次方程为$$\sum _{i=0}^{M-1}{w}_{opt,k}{R}_{u,u}(i-k)\}=R_{u,d}(-k),k=0,1,\mathrm{2...}M-1$$如果定义M个输入向量$$\mathbf{u}(n)=[u(n),u(n-1),..u(n-M+1){]}^T$$那么自相关矩阵为：R=E{u(n)uH(n)}=[Ru,u(0)Ru,u(1)Ru,u(M−1)Ru,u∗(1)Ru,u(0)Ru,u(M−2)⋱Ru,u∗(M−1)Ru,u∗(M−2)Ru,u(0)]R=E\{\bold u(n)\bold u^H(n)\} = \begin{bmatrix} R_{u,u}(0) &R_{u,u}(1) & & R_{u,u}(M-1) \\ R_{u,u}^*(1) &R_{u,u}(0)& & R_{u,u}(M-2) \\ & \ddots & \\ R_{u,u}*(M-1)&R_{u,u}^*(M-2)& &R_{u,u}(0) \end{bmatrix} R=E{u(n)uH(n)}=​Ru,u​(0)Ru,u∗​(1)Ru,u​∗(M−1)​Ru,u​(1)Ru,u​(0)⋱Ru,u∗​(M−2)​​Ru,u​(M−1)Ru,u​(M−2)Ru,u​(0)​​等式右侧则是输入与期望响应的互相关向量：r=E{u(n)d∗(n)}=[Ru,d(0),Ru,d(−1),...,Ru,d(−M+1)]Tr=E\{\bold u(n) d^*(n)\}=[R_{u,d}(0), R_{u,d}(-1),...,R_{u,d}(-M+1)]^Tr=E{u(n)d∗(n)}=[Ru,d​(0),Ru,d​(−1),...,Ru,d​(−M+1)]T进一步简化成矩阵表达：$$R{w}_{opt}=r$$等式两侧左乘以$R^{-1}$，则得到维纳滤波器的最有权重解：$$w_{opt}=R^{-1}r$$

kalman滤波的提出

kalman滤波是再wiener理论上发展的，“采用频域设计法是造成Wiener滤波器设计困难的根本原因。因此人们逐渐转向寻求在时域内直接设计最优滤波器的方法。Kalman在20世纪60年代初提出了Kalman滤波理论。Kalman滤波理论是一种时域方法。它把状态空间的概念引入随机估计理论，把信号过程视为白噪声作用下一个线性系统的输出，用状态方程来描述输入-输出关系，在估计过程中利用系统状态方程、观测方程和白噪声激励，即系统过程噪声和观测噪声，利用它们的统计特性形成滤波算法。由于所用的信息都是时域内的量，所以Kalman滤波不但可以对平稳的一维随机过程进行估计，也可以对非平稳的、多维随机过程进行估计。同时Kalman滤波算法是递推的，便于在计算机上实现实时应用，克服了经典Wiener滤波方法的缺点和局限性”。上面这段摘自《Kalman滤波算法详解及MATLAB实现》相对于wiener解本身的数学方法是非递推的（lms/rls等方法是等效实现），卡尔曼方法先天的递推计算，即自适应滤波器哦。这个推导过程与最小二乘法的自适应框架统一。
考虑一离散时间的动态系统，它由描述状态的过程方程和描述观测向量的观测方程共同表示。
（1）过程方程$$\mathbf{x}(n+1)=\mathbf{F}(n+1,n)\mathbf{x}(n)+{\mathbf{v}}_1(n)$$ $M\ast 1维向量$$x(n)$表示系统再离散时间$n$的状态向量，它是不可观测的；$M\ast M$矩阵$\mathbf{F}(n+1,n)$称为状态转移矩阵，描述动态系统在时间n的状态到n+1的状态之间的转移，它应该是已知的；而$M\ast 1$向量$v_1(n)$的过程噪声向量，描述状态转移中间的加性噪声或误差。因为这样一个过程也可以看作是状态的变迁，所以此方程也称为状态转移方程。
（2）观测方程$$y(n)=\mathbf{C}(n)\mathbf{x}(n)+{\mathbf{v}}_2(n)$$ $y(n)$代表动态系统在时间n的$N\ast 1$观测向量；$N\ast M$矩阵C(n)称为观测矩阵（描述状态经过其作用，变成可观测数据），要求是已知的；$v_2$表示观测噪声向量，其维度与观测向量相同。
要简化计算，以下的假设是必须滴： $v_1(n)$过程噪声向量和$v_2$观测噪声向量都是零均值的白噪声，两者也是线性无关的。他们的相关矩阵分别为E{v1(n)v1H(k)}={Q1(n),if n=kO,if n≠kE\{v_1(n)v_1^H(k)\}=\begin{cases} Q_1(n), & \text{if }n=k \\ O, & \text{if }n\neq k \end{cases}E{v1​(n)v1H​(k)}={Q1​(n),O,​if n=kif n=k​E{v2(n)v2H(k)}={Q2(n),if n=kO,if n≠kE\{v_2(n)v_2^H(k)\}=\begin{cases} Q_2(n), & \text{if }n=k \\ O, & \text{if }n\neq k \end{cases}E{v2​(n)v2H​(k)}={Q2​(n),O,​if n=kif n=k​E{v1(n)v2H(k)}=O,∀n,kE\{v_1(n)v_2^H(k)\}=O, \space \forall n,kE{v1​(n)v2H​(k)}=O, ∀n,k
利用观测的向量$y(1),...,y(n)$对$n\ge 1$求状态向量$\mathbf{x}(i)$各个分量的最小二乘估计，那么具体的根据状态变量索引$i$和观测向量索引$n$的关系，kalman问题又细分为三个类型：
（1）滤波（$i=n$)：使用n时刻及以前时刻的测量数据，抽取n时刻的信息；
（2）平滑（$1\le i<n$)：因为n以前某个时刻的信息，而且n时刻以后的测量数据也可用，非因果的效果使得平滑更加精确；
（3）预测（i > n)：使用n时刻及其以前时刻的测量数据，提前抽取n+$\tau ,\tau >0$时刻（未来）的信息，这是一种预测。

innovation process新息过程

看到这个概念，我的第一反映这是什么WTF？为什么叫“新息”？在卡尔曼滤波中，新息过程就是这个预测值和实际观测值之间的差异。用数学符号表示就是：
$$新息=实际观测值-预测的观测值$$结合上面的符号，给定观测值$y(1),...,y(n-1)$，求观测向量$y(n)$的最小二乘估计，记作$${\hat{y}}_1(n)\stackrel{\text{def}}{=}\hat{y}(n|y(1),...,y(n-1))$$$y(n)$的新息过程定义为$$\alpha (n)=y(n)-{\hat{y}}_1(n)$$$N\ast 1$向量$\alpha (n)$表示观测数据$y(n)$的新的信息，简称新息。新息的性质：
（1）n时刻的新息$\alpha$与过去所有的观测数据$y(1),...,y(n-1)$正交，即E{α(n)yH(k)}=O,1≤k<nE\{\alpha(n)y^H(k)\}=O,\space 1\leq k <nE{α(n)yH(k)}=O, 1≤k<n
（2）新息过程由彼此正交的随机向量序列{α(n)}\{\alpha(n)\}{α(n)}组成，E{α(n)αH(k)}=O,1≤k<nE\{\alpha(n)\alpha^H(k)\}=O,\space 1\leq k <nE{α(n)αH(k)}=O, 1≤k<n
（3）表示观测数据的随机向量序列{y(1),...,y(n)}\{y(1),...,y(n)\}{y(1),...,y(n)}与表示新息过程的随机向量序列{α(1),...,α(n)}\{\alpha(1),...,\alpha(n)\}{α(1),...,α(n)}一一对应，即{y(1),...,y(n)}={α(1),...,α(n)}\{y(1),...,y(n)\}=\{\alpha(1),...,\alpha(n)\}{y(1),...,y(n)}={α(1),...,α(n)}以上性质表明：n时刻的新息$\alpha (n)$是一个与n时刻之前的观测数据{y(1),...,y(n−1)}\{y(1),...,y(n-1)\}{y(1),...,y(n−1)}不相关、具有白噪声性质的随机过程，但它却能够提供有关$y(n)$的新信息。新息的的相关矩阵R(n)=E{α(n)αH(k)}R(n)=E\{\alpha(n)\alpha^H(k)\}R(n)=E{α(n)αH(k)}在卡尔曼滤波中，并不直接估计观测数据向量的一步预测${\hat{y}}_1(n)$，而是先计算状态向量的一步预测：$${\hat{x}}_1(n)\stackrel{\text{def}}{=}x(n|y(1),...,y(n-1))$$然后利用观测方程$${\hat{y}}_1(n)=C(n){\hat{x}}_1(n)$$带入新息计算的公式$$\alpha (n)=y(n)-C(n){\hat{x}}_1(n)=C(n)[x(n)-{\hat{x}}_1(n)]+v_2(n)$$此即新息过程的实际计算公式，但首先一步预测的状态向量估计${\hat{x}}_1(n)$要先求出。定义状态向量的一步预测误差$$ϵ(n,n-1)\stackrel{\text{def}}{=}x(n)-{\hat{x}}_1(n)$$$$\alpha (n)=C(n)ϵ(n,n-1)+v_2(n)$$然后将这个式子带入相关矩阵R(n)=C(n)E{ϵ(n,n−1)ϵH(n,n−1)}CH(n)+E{v2(n)v2H(n)}=C(n)K(n,n−1)CH(n)+Q2(n)R(n)=C(n)E\{\epsilon(n,n-1)\epsilon^H(n,n-1)\}C^H(n)+E\{v_2(n)v_2^H(n)\}\\ =C(n)K(n,n-1)C^H(n)+Q_2(n)R(n)=C(n)E{ϵ(n,n−1)ϵH(n,n−1)}CH(n)+E{v2​(n)v2H​(n)}=C(n)K(n,n−1)CH(n)+Q2​(n)K(n,n−1)=E{ϵ(n,n−1)ϵH(n,n−1)}K(n,n-1)=E\{\epsilon(n,n-1)\epsilon^H(n,n-1)\}K(n,n−1)=E{ϵ(n,n−1)ϵH(n,n−1)}定义为一步预测状态误差的相关矩阵，$Q_2(n)$是观测噪声$v_2(n)$的相关矩阵。

kalman滤波算法

新息过程是为了转入kalman滤波算法的核心问题：如何利用新息过程估计状态向量的预测？答案是用新息过程序列$\alpha (1),...,\alpha (n)$的线性组合直接构造状态向量的一步预测：$${\hat{x}}_1(n+1)\stackrel{\text{def}}{=}{\hat{x}}_1(n+1|y(1),...,y(n))=\sum _{k=1}^n{W}_1(k)\alpha (k)$$式子中$W_1(k)$表示与一步预测相对应的权矩阵，且k为离散时间，如何求解这个权矩阵？那就得利用正交性原理了，即最有估计的误差与已知值正交，这里误差公式是下一拍的，$$ϵ(n+1,n)=x(n+1)-{\hat{x}}_1(n+1)$$，已知值其实就是新息$\alpha (n)$，由此设计公式E{ϵ(n+1,n)αH(k)}=E{[x(n+1)−x^1(n+1)]αH(k)}=O,k=1,...,.nE\{\epsilon(n+1,n)\alpha^H(k)\}=E\{[x(n+1)-\hat x_1(n+1)]\alpha^H(k)\}=O, \space k=1,...,.nE{ϵ(n+1,n)αH(k)}=E{[x(n+1)−x^1​(n+1)]αH(k)}=O, k=1,...,.n代入权重表达式,并利用新息的正交性可得E{x(n+1)αH(k)}=W1(k)E{α(k)αH(k)}=W1(k)R(k)E\{x(n+1)\alpha^H(k)\}=W_1(k)E\{\alpha(k)\alpha^H(k)\}=W_1(k)R(k)E{x(n+1)αH(k)}=W1​(k)E{α(k)αH(k)}=W1​(k)R(k)右×一下$R^{-1}(k)$得到了权重的自洽表达W1(k)=E{x(n+1)αH(k)}R−1(k)W_1(k)=E\{x(n+1)\alpha^H(k)\}R^{-1}(k)W1​(k)=E{x(n+1)αH(k)}R−1(k)由此状态向量的一步预测的最小均方估计表示为x^1(n+1)=∑k=1n−1E{x(n+1)αH(k)}R−1(k)α(k)+E{x(n+1)αH(n)}R−1(n)α(n)\hat x_1(n+1)=\sum_{k=1}^{n-1}E\{x(n+1)\alpha^H(k)\}R^{-1}(k)\alpha(k)+E\{x(n+1)\alpha^H(n)\}R^{-1}(n)\alpha(n)x^1​(n+1)=k=1∑n−1​E{x(n+1)αH(k)}R−1(k)α(k)+E{x(n+1)αH(n)}R−1(n)α(n)利用之前的假设E{v1(n)α(k)}=O,k=0,1,...,nE\{v_1(n)\alpha(k)\}=O, k=0,1,...,nE{v1​(n)α(k)}=O,k=0,1,...,n，结合状态方程$\mathbf{x}(n+1)=\mathbf{F}(n+1,n)\mathbf{x}(n)+{\mathbf{v}}_1(n)$E{x(n+1)αH(k)}=E{[F(n+1,n)x(n)+v1(n)]αH(k)}=F(n+1,n)E{x(n)αH(k)},k=0,1,...,nE\{x(n+1)\alpha^H(k)\}=E\{[\bold F(n+1,n)\bold x(n)+\bold v_1(n)]\alpha^H(k)\}=\bold F(n+1,n)E\{\bold x(n)\alpha^H(k)\}, \space k=0,1,...,nE{x(n+1)αH(k)}=E{[F(n+1,n)x(n)+v1​(n)]αH(k)}=F(n+1,n)E{x(n)αH(k)}, k=0,1,...,n这样先化简最小均方估计的第一项∑k=1n−1E{x(n+1)αH(k)}R−1(k)α(k)=F(n+1,n)∑k=1n−1E{x(n)αH(k)}R−1(k)α(k)=F(n+1,n)x^1(n)\sum_{k=1}^{n-1}E\{x(n+1)\alpha^H(k)\}R^{-1}(k)\alpha(k)=F(n+1,n)\sum_{k=1}^{n-1}E\{x(n)\alpha^H(k)\}R^{-1}(k)\alpha(k)=F(n+1,n)\hat x_1(n)k=1∑n−1​E{x(n+1)αH(k)}R−1(k)α(k)=F(n+1,n)k=1∑n−1​E{x(n)αH(k)}R−1(k)α(k)=F(n+1,n)x^1​(n)最小均方估计的第二项，若定义G(n)=defE{x(n+1)αH(n)}R−1(n)G(n)\stackrel{\text{def}}{=}E\{x(n+1)\alpha^H(n)\}R^{-1}(n)G(n)=defE{x(n+1)αH(n)}R−1(n)那么最终得到的预测公式$$x_1(n+1)==F(n+1,n){\hat{x}}_1(n)+G(n)\alpha (n)$$上面的公式表明n+1时刻的状态向量的一步预测分为非自适应的部分$F(n+1,n){\hat{x}}_1(n)$和自适应的部分$G(n)\alpha (n)$这里$G(n)$称为Kalman增益（矩阵）。初学者到这里肯定微醺了，那不妨先记住流程吧

Kalman 自适应滤波器算法

初始条件

x^1(1)=E{x(1)}\hat x_1(1)=E\{x(1)\}x^1​(1)=E{x(1)}
K(1,0)=E{[x(1)−xˉ(1)][x(1)−xˉ(1)]H},xˉ(1)=E{x(1)}K(1,0)=E\{[x(1)-\bar x(1)][x(1)-\bar x(1)]^H\},\space\space\bar x(1)=E\{x(1)\}K(1,0)=E{[x(1)−xˉ(1)][x(1)−xˉ(1)]H},  xˉ(1)=E{x(1)}

输入观测向量过程

观测向量序列={y(1),...,y(n)}观测向量序列=\{y(1),...,y(n)\}观测向量序列={y(1),...,y(n)}

已知参数

$$\begin{gathered} 状态转移矩阵F(n+1,n) \\ 观测矩阵C(n) \\ 过程噪声向量的相关矩阵Q_1(n) \\ 观测噪声向量的相关矩阵Q_2(n) \end{gathered}$$

计算：n=1，2，3，…

$$\begin{array}{rl}G(n)& =F(n+1,n)K(n,n-1)C^H(n)[C(n)K(n,n-1)C^H(n)+Q_2(n){]}^{-1}\\ \alpha (n)& =y(n)-C(n){\hat{x}}_1(n)\\ {\hat{x}}_1(n+1)& =F(n+1,n){\hat{x}}_1(n)+G(n)\alpha (n)\\ P(n)& =K(n,n-1)-F^{-1}(n+1,n)G(n)C(n)K(n,n-1)\\ K(n+1,n)& =F(n+1,n)P(n)F^H(n+1,n)+Q_1(n)\end{array}$$
上面就是清华专著的推导，Kalman滤波器的估计性能，使滤波后的状态估计误差的相关矩阵P(n)的迹最小化，即Kalman滤波器是状态向量x(n)的线性最小方差估计。

参考

卡尔曼滤波个人学习笔记 (一)
卡尔曼滤波(一)：初始篇
卡尔曼滤波算法原理讲解(搬运的文章，备份留作学习用)
卡尔曼滤波：从入门到精通
图解卡尔曼滤波(Kalman Filter)
The Kalman Filter
Kalman Filter 卡尔曼滤波
A geometric interpretation of the covariance matrix
How a Kalman filter works, in pictures
卡尔曼滤波原理及应用——MATLAB仿真
Kalman滤波算法详解及MATLAB实现