站在哪个肩膀上开始学习卡尔曼滤波

站在哪个肩膀上开始学习卡尔曼滤波

• 前言
• 从自适应滤波的角度
• • 正交性原理到维纳解
  • kalman滤波的提出
  • innovation process新息过程
  • kalman滤波算法
  • Kalman 自适应滤波器算法
  • • 初始条件
    • 输入观测向量过程
    • 已知参数
    • 计算：n=1，2，3，..
• 参考

前言

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

从自适应滤波的角度

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

正交性原理到维纳解

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

kalman滤波的提出

kalman滤波是再wiener理论上发展的，“采用频域设计法是造成Wiener滤波器设计困难的根本原因。因此人们逐渐转向寻求在时域内直接设计最优滤波器的方法。Kalman在20世纪60年代初提出了Kalman滤波理论。Kalman滤波理论是一种时域方法。它把状态空间的概念引入随机估计理论，把信号过程视为白噪声作用下一个线性系统的输出，用状态方程来描述输入-输出关系，在估计过程中利用系统状态方程、观测方程和白噪声激励，即系统过程噪声和观测噪声，利用它们的统计特性形成滤波算法。由于所用的信息都是时域内的量，所以Kalman滤波不但可以对平稳的一维随机过程进行估计，也可以对非平稳的、多维随机过程进行估计。同时Kalman滤波算法是递推的，便于在计算机上实现实时应用，克服了经典Wiener滤波方法的缺点和局限性”。上面这段摘自《Kalman滤波算法详解及MATLAB实现》相对于wiener解本身的数学方法是非递推的（lms/rls等方法是等效实现），卡尔曼方法先天的递推计算，即自适应滤波器哦。这个推导过程与最小二乘法的自适应框架统一。
考虑一离散时间的动态系统，它由描述状态的过程方程和描述观测向量的观测方程共同表示。
（1）过程方程 x ( n + 1 ) = F ( n + 1 , n ) x ( n ) + v 1 ( n ) \\bold x(n+1)=\\bold F(n+1,n)\\bold x(n)+\\bold v_1(n) x(n+1)=F(n+1,n)x(n)+v1​(n) $M\ast 1维向量$$x(n)$表示系统再离散时间$n$的状态向量，它是不可观测的；$M\ast M$矩阵$\mathbf{F}(n+1,n)$称为状态转移矩阵，描述动态系统在时间n的状态到n+1的状态之间的转移，它应该是已知的；而$M\ast 1$向量 v 1 (n) v_1(n) v1​(n)的过程噪声向量，描述状态转移中间的加性噪声或误差。因为这样一个过程也可以看作是状态的变迁，所以此方程也称为状态转移方程。
（2）观测方程 y ( n ) = C ( n ) x ( n ) + v 2 ( n ) y(n)=\\bold C(n)\\bold x(n)+\\bold v_2(n) y(n)=C(n)x(n)+v2​(n) $y(n)$代表动态系统在时间n的$N\ast 1$观测向量；$N\ast M$矩阵C(n)称为观测矩阵（描述状态经过其作用，变成可观测数据），要求是已知的； v 2 v_2 v2​表示观测噪声向量，其维度与观测向量相同。
要简化计算，以下的假设是必须滴： v 1 (n) v_1(n) v1​(n)过程噪声向量和 v 2 v_2 v2​观测噪声向量都是零均值的白噪声，两者也是线性无关的。他们的相关矩阵分别为 E { v 1 ( n ) v 1 H ( k ) } = { Q 1 ( n ) , if  n = k O , if  n ≠ k E\\{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 { v 2 ( n ) v 2 H ( k ) } = { Q 2 ( n ) , if  n = k O , if  n ≠ k E\\{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 { v 1 ( n ) v 2 H ( k ) } = O ,   ∀ n , k E\\{v_1(n)v_2^H(k)\\}=O, \\space \\forall n,k E{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)$的最小二乘估计，记作 y ^ 1 ( n ) = def y ^ ( n ∣ y ( 1 ) , . . . , y ( n − 1 ) ) \\hat y_1(n)\\stackrel{\\text{def}}{=}\\hat y(n|y(1),...,y(n-1)) y^​1​(n)=defy^​(n∣y(1),...,y(n−1))$y(n)$的新息过程定义为 α ( n ) = y ( n ) − y ^ 1 ( n ) \\alpha(n)=y(n)-\\hat y_1(n) α(n)=y(n)−y^​1​(n)$N\ast 1$向量$\alpha (n)$表示观测数据$y(n)$的新的信息，简称新息。新息的性质：
（1）n时刻的新息$\alpha$与过去所有的观测数据$y(1),...,y(n-1)$正交，即 E { α ( n ) y H ( k ) } = O ,   1 ≤ k < n E\\{\\alpha(n)y^H(k)\\}=O,\\space 1\\leq k <n E{α(n)yH(k)}=O, 1≤k<n
（2）新息过程由彼此正交的随机向量序列 {α(n)} \\{\\alpha(n)\\} {α(n)}组成， E { α ( n ) α H ( k ) } = O ,   1 ≤ k < n E\\{\\alpha(n)\\alpha^H(k)\\}=O,\\space 1\\leq k <n E{α(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)}在卡尔曼滤波中，并不直接估计观测数据向量的一步预测 y ^ 1 (n) \\hat y_1(n) y^​1​(n)，而是先计算状态向量的一步预测： x ^ 1 ( n ) = defx ( n ∣ y ( 1 ) , . . . , y ( n − 1 ) ) \\hat x_1(n)\\stackrel{\\text{def}}{=} x(n|y(1),...,y(n-1)) x^1​(n)=defx(n∣y(1),...,y(n−1))然后利用观测方程 y ^ 1 ( n ) = C ( n ) x ^ 1 ( n ) \\hat y_1(n)=C(n)\\hat x_1(n) y^​1​(n)=C(n)x^1​(n)带入新息计算的公式 α ( n ) = y ( n ) − C ( n ) x ^ 1 ( n ) = C ( n ) [ x ( n ) − x ^ 1 ( n ) ] + v 2 ( n ) \\alpha(n)=y(n)-C(n)\\hat x_1(n)=C(n)[x(n)-\\hat x_1(n)]+v_2(n) α(n)=y(n)−C(n)x^1​(n)=C(n)[x(n)−x^1​(n)]+v2​(n)此即新息过程的实际计算公式，但首先一步预测的状态向量估计 x ^ 1 (n) \\hat x_1(n) x^1​(n)要先求出。定义状态向量的一步预测误差 ϵ ( n , n − 1 ) = defx ( n ) − x ^ 1 ( n ) \\epsilon(n,n-1)\\stackrel{\\text{def}}{=}x(n)-\\hat x_1(n) ϵ(n,n−1)=defx(n)−x^1​(n) α ( n ) = C ( n ) ϵ ( n , n − 1 ) + v 2 ( n ) \\alpha(n)=C(n)\\epsilon(n,n-1)+v_2(n) α(n)=C(n)ϵ(n,n−1)+v2​(n)然后将这个式子带入相关矩阵 R ( n ) = C ( n ) E { ϵ ( n , n − 1 ) ϵ 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\\{\\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) Q_2(n) Q2​(n)是观测噪声 v 2 (n) v_2(n) v2​(n)的相关矩阵。

kalman滤波算法

新息过程是为了转入kalman滤波算法的核心问题：如何利用新息过程估计状态向量的预测？答案是用新息过程序列$\alpha (1),...,\alpha (n)$的线性组合直接构造状态向量的一步预测： x ^ 1 ( n + 1 ) = def x ^ 1 ( n + 1 ∣ y ( 1 ) , . . . , y ( n ) ) = ∑ k = 1 nW 1 ( k ) α ( k ) \\hat x_1(n+1)\\stackrel{\\text{def}}{=}\\hat x_1(n+1|y(1),...,y(n)) =\\sum_{k=1}^nW_1(k)\\alpha(k) x^1​(n+1)=defx^1​(n+1∣y(1),...,y(n))=k=1∑n​W1​(k)α(k)式子中 W 1 (k) W_1(k) W1​(k)表示与一步预测相对应的权矩阵，且k为离散时间，如何求解这个权矩阵？那就得利用正交性原理了，即最有估计的误差与已知值正交，这里误差公式是下一拍的， ϵ ( n + 1 , n ) = x ( n + 1 ) − x ^ 1 ( n + 1 ) \\epsilon(n+1,n){=}x(n+1)-\\hat x_1(n+1) ϵ(n+1,n)=x(n+1)−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 , . . . , . n E\\{\\epsilon(n+1,n)\\alpha^H(k)\\}=E\\{[x(n+1)-\\hat x_1(n+1)]\\alpha^H(k)\\}=O, \\space k=1,...,.n E{ϵ(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 ) } = W 1 ( k ) E { α ( k ) α H ( k ) } = W 1 ( 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) R^{-1}(k) R−1(k)得到了权重的自洽表达 W 1 ( 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 = 1 n − 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{ v 1 (n)α(k)}=O,k=0,1,...,n E\\{v_1(n)\\alpha(k)\\}=O, k=0,1,...,n E{v1​(n)α(k)}=O,k=0,1,...,n，结合状态方程 x(n+1)=F(n+1,n)x(n)+ v 1 (n) \\bold x(n+1)=\\bold F(n+1,n)\\bold x(n)+\\bold v_1(n) x(n+1)=F(n+1,n)x(n)+v1​(n) E { x ( n + 1 ) α H ( k ) } = E { [ F ( n + 1 , n ) x ( n ) + v 1 ( n ) ] α H ( k ) } = F ( n + 1 , n ) E { x ( n ) α H ( k ) } ,   k = 0 , 1 , . . . , n E\\{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,...,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,...,n这样先化简最小均方估计的第一项 ∑ k = 1 n − 1E { x ( n + 1 ) α H ( k ) } R − 1( k ) α ( k ) = F ( n + 1 , n ) ∑ k = 1 n − 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 ) x ^ 1 ( n ) + G ( n ) α ( n ) x_1(n+1)==F(n+1,n)\\hat x_1(n)+G(n)\\alpha(n) x1​(n+1)==F(n+1,n)x^1​(n)+G(n)α(n)上面的公式表明n+1时刻的状态向量的一步预测分为非自适应的部分 F(n+1,n) x ^ 1 (n) F(n+1,n)\\hat x_1(n) F(n+1,n)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)}

已知参数

状态转移矩阵 F ( n + 1 , n ) 观测矩阵 C ( n ) 过程噪声向量的相关矩阵 Q 1 ( n ) 观测噪声向量的相关矩阵 Q 2 ( n ) 状态转移矩阵F(n+1,n)\\\\观测矩阵C(n)\\\\过程噪声向量的相关矩阵Q_1(n) \\\\观测噪声向量的相关矩阵Q_2(n) 状态转移矩阵F(n+1,n)观测矩阵C(n)过程噪声向量的相关矩阵Q1​(n)观测噪声向量的相关矩阵Q2​(n)

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

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 α ( n ) = y ( n ) − C ( n )x ^ 1 ( n ) x ^ 1 ( n + 1 ) = F ( n + 1 , n )x ^ 1 ( n ) + G ( n ) α ( 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 ) \\begin{aligned} 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{aligned} G(n)α(n)x^1​(n+1)P(n)K(n+1,n)​=F(n+1,n)K(n,n−1)CH(n)[C(n)K(n,n−1)CH(n)+Q2​(n)]−1=y(n)−C(n)x^1​(n)=F(n+1,n)x^1​(n)+G(n)α(n)=K(n,n−1)−F−1(n+1,n)G(n)C(n)K(n,n−1)=F(n+1,n)P(n)FH(n+1,n)+Q1​(n)​
上面就是清华专著的推导，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实现