关注我的公众号c137Lab获取更多相关内容
本文中的工程开源在GitHub
卡尔曼滤波
卡尔曼滤波以其发明者鲁道夫.E.卡尔曼（Rudolph E. Kalman）命名，但根据文献可知卡尔曼并非第一个提出该算法的人，Peter Swerling在更早之前就提出了类似算法1。
卡尔曼滤波的典型实例是从一组有限、包含噪声的观测序列中预测出物体的位置和速度。最为常见的卡尔曼滤波器是锁相环。
线性卡尔曼滤波
待估计模型
卡尔曼滤波建立在隐马尔可夫模型上，其基本动态系统使用一个马尔可夫链表示。
![![img1][img1]](https://img-blog.csdnimg.cn/20201026151931195.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3ZlcnNlX21vbmdlcg==,size_16,color_FFFFFF,t_70#pic_center)
使用公式表示即，对于需要求解的状态 x ∈ R n x\in R^n x∈Rn 满足
x k = F k x k − 1 + B k u k − 1 + w k − 1 x_k=F_kx_{k-1}+B_ku_{k-1}+w_{k-1} xk=Fkxk−1+Bkuk−1+wk−1
其中 F F F 为 n × n n\times n n×n 状态的转移矩阵，在现实的过程中他可能是时变的，但是在这里假设其为常量。 n × l n\times l n×l 矩阵 B B B 是可能存在的控制器 u ∈ R l u\in R^l u∈Rl 的输入-控制模型。对于这一隐马尔可夫链有观测值 z ∈ R m z\in R^m z∈Rm 满足
z k = H k x k + v k z_k=H_kx_k+v_k zk=Hkxk+vk
其中 m × n m\times n m×n 矩阵 H H H 为观测模型，在真实情况中观测模型可能会是时变的，但在此处假设其为常量。
随机变量 w k , v k w_k,v_k wk,vk 分别为过程噪声和观测噪声，假设他们是独立的高斯白噪声且
p ( w k ) ∼ N ( 0 , Q k ) p(w_k)\sim N(0,Q_k) p(wk)∼N(0,Qk)
p ( v k ) ∼ N ( 0 , R k ) p(v_k)\sim N(0,R_k) p(vk)∼N(0,Rk)
推导
卡尔曼滤波器是建立在贝叶斯滤波器的基础上的2，首先介绍贝叶斯滤波器。
贝叶斯准则与全概率公式
贝叶斯准则：
p ( x ∣ y , z ) = p ( y ∣ x , z ) p ( x ∣ z ) p ( y ∣ z ) p(x|y,z)=\frac{p(y|x,z)p(x|z)}{p(y|z)} p(x∣y,z)=p(y∣z)p(y∣x,z)p(x∣z)
全概率公式：
p ( x ) = ∫ p ( x ∣ y ) p ( y ) d y p(x)=\int p(x|y)p(y)dy p(x)=∫p(x∣y)p(y)dy
贝叶斯滤波器
贝叶斯滤波器的思想即使用已有的观测值和控制输入来估计当前时刻的状态，对于状态 x k x_k xk ，观测值 z k z_k zk ，控制量 u k u_k uk，写成概率的形式即：
b e l ( x k ) = p ( x k ∣ z 1 : k , u 1 : k ) = p ( x k ∣ z k , z 1 : k − 1 , u 1 : k ) bel(x_k)=p(x_k|z_{1:k},u_{1:k})=p(x_k|z_k,z_{1:k-1},u_{1:k}) bel(xk)=p(xk∣z1:k,u1:k)=p(xk∣zk,z1:k−1,u1:k)
根据贝叶斯准则
p ( x k ∣ z 1 : k , u 1 : k ) = p ( z k ∣ x k , z 1 : k − 1 , u 1 : k ) p ( x k ∣ z 1 : k − 1 , u 1 : k ) p ( z k ∣ z 1 : k − 1 , u 1 : k ) p(x_k|z_{1:k},u_{1:k})=\frac{p(z_k|x_k,z_{1:k-1},u_{1:k})p(x_k|z_{1:k-1},u_{1:k})}{p(z_k|z_{1:k-1},u_{1:k})} p(xk∣z1:k,u1:k)=p(zk∣z1:k−1,u1:k)p(zk∣xk,z1:k−1,u1:k)p(xk∣z1:k−1,u1:k)
分母与 x k x_k xk 无关，使用常数 1 / η 1/\eta 1/η 代替，由于隐马尔可夫链的性质，某时刻的观测值只与其状态有关，则 p ( z k ∣ x k , z 1 : k − 1 , u 1 : k ) = p ( z k ∣ x k ) p(z_k|x_k,z_{1:k-1},u_{1:k})=p(z_k|x_k) p(zk∣xk,z1:k−1,u1:k)=p(zk∣xk) 上式可以化简为：
p ( x k ∣ z 1 : k , u 1 : k ) = η p ( z k ∣ x k ) p ( x k ∣ z 1 : k − 1 , u 1 : k ) p(x_k|z_{1:k},u_{1:k})=\eta p(z_k|x_k)p(x_k|z_{1:k-1},u_{1:k}) p(xk∣z1:k,u1:k)=ηp(zk∣xk)p(xk∣z1:k−1,u1:k)
可以理解为使用先验和似然估计后验。则先验为：
b e l ‾ ( x k ) = p ( x k ∣ z 1 : k − 1 , u 1 : k ) \overline{bel}(x_k)=p(x_k|z_{1:k-1},u_{1:k}) bel(xk)=p(xk∣z1:k−1,u1:k)
代入全概率公式，由于积分中乘积的前一项为根据前一时刻状态预测下一时刻状态，根据马尔可夫链的性质，这一概率与观测值 z z z 无关。同时，马尔可夫性质表明只有当前的控制信号和上一时刻的状态对当前时刻的状态有影响，则积分乘积前一项中的 u 1 : k u_{1:k} u1:k 可以被化为 u k u_k uk ，乘积后一项中的 u 1 : k u_{1:k} u1:k 可以被化为 u 1 : k − 1 u_{1:k-1} u1:k−1。同时观察可知乘积的第二项即 b e l ( x k − 1 ) bel(x_{k-1}) bel(xk−1)：
b e l ‾ ( x k ) = ∫ p ( x k ∣ x k − 1 , z 1 : k − 1 , u 1 : k ) p ( x k − 1 ∣ z 1 : k − 1 , u 1 : k ) d x k − 1 = ∫ p ( x k ∣ x k − 1 , u k ) b e l ( x k − 1 ) d x k − 1 \overline{bel}(x_k)=\int p(x_k|x_{k-1},z_{1:k-1},u_{1:k})p(x_{k-1}|z_{1:k-1},u_{1:k})dx_{k-1}\\=\int p(x_k|x_{k-1},u_k)bel(x_{k-1})dx_{k-1} bel(xk)=∫p(xk∣xk−1,z1:k−1,u1:k)p(xk−1∣z1:k−1,u1:k)dxk−1=∫p(xk∣xk−1,uk)bel(xk−1)dxk−1
后验为：
b e l ( x k ) = η p ( z k ∣ x k ) b e l ‾ ( x k ) bel(x_k)=\eta p(z_k|x_k)\overline{bel}(x_k) bel(xk)=ηp(zk∣xk)bel(xk)
代入待估计模型
求解先验分布
由于假设噪声符合高斯分布，则 b e l ( x k ) bel(x_k) bel(xk) 也应符合高斯分布，设其均值为 μ k \mu_k μk ，方差 Σ k \Sigma_k Σk 。对于先验公式，有
p ( x k ∣ x k − 1 , u k ) ∼ N ( F k x k − 1 + B k u k , Q k ) b e l ( x k − 1 ) ∼ N ( μ k − 1 , Σ k − 1 ) p(x_k|x_{k-1},u_k)\sim N(F_kx_{k-1}+B_ku_k,Q_k)\\ bel(x_{k-1})\sim N(\mu_{k-1},\Sigma_{k-1}) p(xk∣xk−1,uk)∼N(Fkxk−1+Bkuk,Qk)bel(xk−1)∼N(μk−1,Σk−1)
则 b e l ‾ ( x k ) \overline{bel}(x_k) bel(xk) 可以表示成 b e l ‾ ( x k ) = γ ∫ e x p ( − L k ) d x k − 1 \overline{bel}(x_k)=\gamma\int exp(-L_k)dx_{k-1} bel(xk)=γ∫exp(−Lk)dxk−1 的形式，其中 γ \gamma γ 为常数，有
L k = 1 2 [ ( x k − F k x k − 1 − B k u k ) T Q k − 1 ( x k − F k x k − 1 − B k u k ) + ( x k − 1 − μ k − 1 ) T Σ k − 1 − 1 ( x k − 1 − μ k − 1 ) ] L_k=\frac12[(x_k-F_kx_{k-1}-B_ku_k)^TQ_k^{-1}(x_k-F_kx_{k-1}-B_ku_k)+(x_{k-1}-\mu_{k-1})^T\Sigma_{k-1}^{-1}(x_{k-1}-\mu_{k-1})] Lk=21[(xk−Fkxk−1−Bkuk)TQk−1(xk−Fkxk−1−Bkuk)+(xk−1−μk−1)TΣk−1−1(xk−1−μk−1)]
两个高斯分布的乘积 γ e x p ( − L k ) \gamma exp(-L_k) γexp(−Lk) 也为高斯分布，而 L k L_k Lk 是关于 x k − 1 x_{k-1} xk−1 的二次型，则求解 ∂ L k / ∂ x k − 1 = 0 \partial L_k/\partial x_{k-1}=0 ∂Lk/∂xk−1=0 可以得到该分布的均值 μ k ′ \mu_k' μk′， ∂ 2 L k / ∂ x k − 1 2 \partial^2L_k/\partial x_{k-1}^2 ∂2Lk/∂xk−12 为其方差的倒数 Ψ k − 1 \Psi_k^{-1} Ψk−1。可以解得：
μ k ′ = F k T Q k − 1 x k − F k T Q k − 1 B k u k + Σ k − 1 − 1 μ k − 1 Σ k − 1 − 1 + F k T Q k − 1 F k Ψ k − 1 = F k T Q k − 1 F k + Σ k − 1 − 1 \mu_k'=\frac{F_k^TQ_k^{-1}x_k-F_k^TQ_k^{-1}B_ku_k+\Sigma_{k-1}^{-1}\mu_{k-1}}{\Sigma_{k-1}^{-1}+F_k^TQ_k^{-1}F_k}\\ \Psi^{-1}_k=F_k^TQ_k^{-1}F_k+\Sigma_{k-1}^{-1} μk′=Σk−1−1+FkTQk−1FkFkTQk−1xk−FkTQk−1Bkuk+Σk−1−1μk−1Ψk−1=FkTQk−1Fk+Σk−1−1
为了求解积分，需要使用一些技巧来简化运算，定义二次型函数
L k ′ ( x k − 1 , x k ) = 1 2 ( x k − 1 − μ k ′ ) T Ψ k − 1 ( x k − 1 − μ k ′ ) L_k'(x_{k-1},x_k)=\frac12(x_{k-1}-\mu_k')^T\Psi_k^{-1}(x_{k-1}-\mu_k') Lk′(xk−1,xk)=21(xk−1−μk′)TΨk−1(xk−1−μk′)
可以发现 L k − L k ′ L_k-L'_k Lk−Lk′ 为 x k x_k xk 的二次型，且有 d e t ( 2 π Ψ k ) 1 / 2 ∫ e x p ( − L k ′ ) d x t − 1 = 1 det(2\pi \Psi_k)^{1/2}\int exp(-L_k')dx_{t-1}=1 det(2πΨk)1/2∫exp(−Lk′)dxt−1=1 ，则可以将 L k L_k Lk 拆分来去积分。定义
L k ′ ′ ( x k ) = L k − L k ′ L_k''(x_k)=L_k-L_k' Lk′′(xk)=Lk−Lk′
则
b e l ‾ ( x k ) = γ ∫ e x p [ − L k ′ ′ ( x k ) − L k ′ ( x k − 1 , x k ) ] d x t − 1 = γ e x p [ − L k ′ ′ ( x k ) ] \overline{bel}(x_k)=\gamma\int exp[-L_k''(x_k)-L_k'(x_{k-1},x_k)]dx_{t-1}\\ =\gamma exp[-L_k''(x_k)] bel(xk)=γ∫exp[−Lk′′(xk)−Lk′(xk−1,xk)]dxt−1=γexp[−Lk′′(xk)]
由于 L k ′ ′ L''_k Lk′′ 为关于 x k x_k xk 的二次型，且 b e l ‾ ( x k ) \overline{bel}(x_k) bel(xk) 符合高斯分布，则使用求一阶导数和二阶导数的方法来求取其方差 Σ ‾ k \overline\Sigma_k Σk 和均值 μ ‾ k \overline\mu_k μk。
一阶导数：
∂ L k ′ ′ ( x k ) ∂ x k = Q k − 1 ( x k − F k x k − 1 − B k u k ) + Q k − 1 F k Σ k − 1 − 1 + F k T Q k − 1 F k ( F k T Q k − 1 F k + Σ k − 1 − 1 ) ( x k − 1 − μ k ′ ) = Q k − 1 ( x k − F k x k − 1 − B k u k ) + Q k − 1 F k ( x k − 1 − μ k ′ ) = Q k − 1 ( x k − B k u k ) − Q k − 1 F k μ k ′ = Q k − 1 ( x k − B k u k ) − Q k − 1 F k F k T Q k − 1 ( x k − B k u k ) + Σ k − 1 − 1 μ k − 1 Σ k − 1 − 1 + F k T Q k − 1 F k = ( Q k − 1 − Q k − 1 F k F k T Q k − 1 Σ k − 1 − 1 + F k T Q k − 1 F k ) ( x k − B k u k ) − Q k − 1 F k Σ k − 1 − 1 μ k − 1 Σ k − 1 − 1 + F k T Q k − 1 F k ( 求 逆 公 式 ) = ( Q k + F k Σ k − 1 F k T ) − 1 ( x k − B k u k ) − Q k − 1 F k Σ k − 1 − 1 μ k − 1 Σ k − 1 − 1 + F k T Q k − 1 F k \frac{\partial L_k''(x_k)}{\partial x_k}=Q_k^{-1}(x_k-F_kx_{k-1}-B_ku_k)+\frac{Q_k^{-1}F_k}{\Sigma_{k-1}^{-1}+F_k^TQ_k^{-1}F_k}(F_k^TQ_k^{-1}F_k+\Sigma_{k-1}^{-1})(x_{k-1}-\mu'_k)\\ =Q_k^{-1}(x_k-F_kx_{k-1}-B_ku_k)+Q_k^{-1}F_k(x_{k-1}-\mu'_k)\\ =Q_k^{-1}(x_k-B_ku_k)-Q_k^{-1}F_k\mu'_k\\ =Q_k^{-1}(x_k-B_ku_k)-Q_k^{-1}F_k\frac{F_k^TQ_k^{-1}(x_k-B_ku_k)+\Sigma_{k-1}^{-1}\mu_{k-1}}{\Sigma_{k-1}^{-1}+F_k^TQ_k^{-1}F_k}\\ =(Q_k^{-1}-Q_k^{-1}F_k\frac{F_k^TQ_k^{-1}}{\Sigma_{k-1}^{-1}+F_k^TQ_k^{-1}F_k})(x_k-B_ku_k)-Q_k^{-1}F_k\frac{\Sigma_{k-1}^{-1}\mu_{k-1}}{
{\Sigma_{k-1}^{-1}+F_k^TQ_k^{-1}F_k}}\quad(求逆公式)\\ =(Q_k+F_k\Sigma_{k-1}F_k^T)^{-1}(x_k-B_ku_k)-Q_k^{-1}F_k\frac{\Sigma_{k-1}^{-1}\mu_{k-1}}{
{\Sigma_{k-1}^{-1}+F_k^TQ_k^{-1}F_k}} ∂xk∂Lk′′(xk)=Qk−1(xk−Fkxk−1−Bkuk)+Σk−1−1+FkTQk−1FkQk−1Fk(FkTQk−1Fk+Σk−1−1)(xk−1−μk′)=Qk−1(xk−Fkxk−1−Bkuk)+Qk−1Fk(xk−1−μk′)=Qk−1(xk−Bkuk)−Qk−1Fkμk′=Qk−1(xk−Bkuk)−Qk−1FkΣk−1−1+FkTQk−1FkFkTQk−1(xk−Bkuk)+Σk−1−1μk−1=(Qk−1−Qk−1FkΣk−1−1+FkTQk−1FkFkTQk−1)(xk−Bkuk)−Qk−1FkΣk−1−1+FkTQk−1FkΣk−1−1μk−1(求逆公式)=(Qk+FkΣk−1FkT)−1(xk−Bkuk)−Qk−1FkΣk−1−1+FkTQk−1FkΣk−1−1μk−1
令一阶导数等于零
∂ L k ′ ′ ( x k ) ∂ x k = 0 ( Q k + F k Σ k − 1 F k T ) − 1 ( x k − B k u k ) = Q k − 1 F k Σ k − 1 − 1 μ k − 1 Σ k − 1 − 1 + F k T Q k − 1 F k x k − B k u k = ( Q k + F k Σ k − 1 F k T ) Q k − 1 F k Σ k − 1 − 1 μ k − 1 Σ k − 1 − 1 + F k T Q k − 1 F k x k − B k u k = ( Q k + F k Σ k − 1 F k T ) Q k − 1 F k ( Σ k − 1 − 1 + F k T Q k − 1 F k ) − 1 Σ k − 1 − 1 μ k − 1 x k − B k u k = ( Q k + F k Σ k − 1 F k T ) Q k − 1 F k ( I + Σ k − 1 F k T Q k − 1 F k ) − 1 μ k − 1 x k − B k u k = ( F k + F k Σ k − 1 F k T Q k − 1 F k ) ( I + Σ k − 1 F k T Q k − 1 F k ) − 1 μ k − 1 x k − B k u k = F k ( I + Σ k − 1 F k T Q k − 1 F k ) ( I + Σ k − 1 F k T Q k − 1 F k ) − 1 μ k − 1 x k − B k u k = F k μ k − 1 x k = F k u k − 1 + B k u k \frac{\partial L_k''(x_k)}{\partial x_k}=0\\ (Q_k+F_k\Sigma_{k-1}F_k^T)^{-1}(x_k-B_ku_k)=Q_k^{-1}F_k\frac{\Sigma_{k-1}^{-1}\mu_{k-1}}{
{\Sigma_{k-1}^{-1}+F_k^TQ_k^{-1}F_k}}\\ x_k-B_ku_k=(Q_k+F_k\Sigma_{k-1}F_k^T)Q_k^{-1}F_k\frac{\Sigma_{k-1}^{-1}\mu_{k-1}}{
{\Sigma_{k-1}^{-1}+F_k^TQ_k^{-1}F_k}}\\ x_k-B_ku_k=(Q_k+F_k\Sigma_{k-1}F_k^T)Q_k^{-1}F_k(\Sigma_{k-1}^{-1}+F_k^TQ_k^{-1}F_k)^{-1}\Sigma_{k-1}^{-1}\mu_{k-1}\\ x_k-B_ku_k=(Q_k+F_k\Sigma_{k-1}F_k^T)Q_k^{-1}F_k(I+\Sigma_{k-1}F_k^TQ_k^{-1}F_k)^{-1}\mu_{k-1}\\ x_k-B_ku_k=(F_k+F_k\Sigma_{k-1}F_k^TQ_k^{-1}F_k)(I+\Sigma_{k-1}F_k^TQ_k^{-1}F_k)^{-1}\mu_{k-1}\\ x_k-B_ku_k=F_k(I+\Sigma_{k-1}F_k^TQ_k^{-1}F_k)(I+\Sigma_{k-1}F_k^TQ_k^{-1}F_k)^{-1}\mu_{k-1}\\ x_k-B_ku_k=F_k\mu_{k-1}\\ x_k=F_ku_{k-1}+B_ku_k ∂xk∂Lk′′(xk)=0(Qk+FkΣk−1FkT)−1(xk−Bkuk)=Qk−1FkΣk−1−1+FkTQk−1FkΣk−1−1μk−1xk−Bkuk=(Qk+FkΣk−1FkT)Qk−1FkΣk−1−1+FkTQk−1FkΣk−1−1μk−1xk−Bkuk=(Qk+FkΣk−1FkT)Qk−1Fk(Σk−1−1+FkTQk−1Fk)−1Σk−1−1μk−1xk−Bkuk=(Qk+FkΣk−1FkT)Qk−1Fk(I+Σk−1FkTQk−1Fk)−1μk−1xk−Bkuk=(Fk+FkΣk−1FkTQk−1Fk)(I+Σk−1FkTQk−1Fk)−1μk−1xk−Bkuk=Fk(I+Σk−1FkTQk−1Fk)(I+Σk−1FkTQk−1Fk)−1μk−1xk−Bkuk=Fkμk−1xk=Fkuk−1+Bkuk
则
μ ‾ k = F k u k − 1 + B k u k (1) \overline\mu_k=F_ku_{k-1}+B_ku_k\tag1 μk=Fkuk−1+Bkuk(1)
二阶导数：
Σ ‾ k − 1 = ∂ 2 L k ′ ′ ( x k ) ∂ x k 2 = ( Q k + F k Σ k − 1 F k T ) − 1 (2) \overline\Sigma_k^{-1}=\frac{\partial^2L_k''(x_k)}{\partial x_k^2}=(Q_k+F_k\Sigma_{k-1}F_k^T)^{-1}\tag2 Σk−1=∂xk2∂2Lk′′(xk)=(Qk+FkΣk−1FkT)−1(2)
则得到先验分布 b e l ‾ ( x k ) ∼ N ( μ ‾ k , Σ ‾ k ) \overline{bel}(x_k)\sim N(\overline\mu_k,\overline\Sigma_k) bel(xk)∼N(μk,Σk)
求解后验分布
由系统的观测方程可以得到 p ( z k ∣ x k ) ∼ N ( H k x k , R k ) p(z_k|x_k)\sim N(H_kx_k,R_k) p(zk∣xk)∼N(Hkxk,Rk) ，已经求得先验分布 b e l ‾ ( x k ) ∼ N ( μ ‾ k , Σ ‾ k ) \overline{bel}(x_k)\sim N(\overline\mu_k,\overline\Sigma_k) bel(xk)∼N(μk,Σk)，则后验分布
b e l ( x k ) = η p ( z k ∣ x k ) b e l ‾ ( x k ) bel(x_k)=\eta p(z_k|x_k)\overline {bel}(x_k) bel(xk)=ηp(zk∣xk)bel(xk)
可以简化为 b e l ( x k ) = η e x p ( − J k ) bel(x_k)=\eta exp(-J_k) bel(xk)=ηexp(−Jk) ，其中
J k = 1 2 [ ( z k − H k x k ) T R k − 1 ( z k − H k x k ) + ( x k − μ ‾ k ) T Σ ‾ k − 1 ( x k − μ ‾ k ) ] J_k=\frac12[(z_k-H_kx_k)^TR_k^{-1}(z_k-H_kx_k)+(x_k-\overline\mu_k)^T\overline\Sigma_k^{-1}(x_k-\overline\mu_k)] Jk=21[(zk−Hkxk)TRk−1(zk−Hkxk)+(xk−μk)TΣk−1(xk−μk)]
由于 J k J_k Jk 为 x k x_k xk 的二次型，同时 b e l ( x k ) bel(x_k) bel(xk) 符合高斯分布，继续通过求解一阶导数和二阶导数的方法求均值和方差。
一阶导数：
∂ J k ∂ x k = − H k T R k − 1 ( z k − H k x k ) + Σ ‾ k − 1 ( x k − μ ‾ k ) \frac{\partial J_k}{\partial x_k}=-H_k^TR_k^{-1}(z_k-H_kx_k)+\overline\Sigma_k^{-1}(x_k-\overline\mu_k) ∂xk∂Jk=−HkTRk−1(zk−Hkxk)+Σk−1(xk−μk)
令一阶导数等于零
∂ J k ∂ x k = 0 H k T R k − 1 ( z k − H k x k ) = Σ ‾ k − 1 ( x k − μ ‾ k ) ( H k T R k − 1 H k + Σ ‾ t − 1 ) x k = H k T R k − 1 z k + Σ ‾ k − 1 μ ‾ k x k = μ ‾ k + H k T R k − 1 H k T R k − 1 H k + Σ ‾ k − 1 ( z k − H k μ ‾ k ) \frac{\partial J_k}{\partial x_k}=0\\ H_k^TR_k^{-1}(z_k-H_kx_k)=\overline\Sigma_k^{-1}(x_k-\overline\mu_k)\\ (H_k^TR_k^{-1}H_k+\overline\Sigma_t^{-1})x_k=H_k^TR_k^{-1}z_k+\overline\Sigma_k^{-1}\overline\mu_k\\ x_k=\overline\mu_k+\frac{H_k^TR_k^{-1}}{H_k^TR_k^{-1}H_k+\overline\Sigma_k^{-1}}(z_k-H_k\overline\mu_k) ∂xk∂Jk=0HkTRk−1(zk−Hkxk)=Σk−1(xk−μk)(HkTRk−1Hk+Σt−1)xk=HkTRk−1zk+Σk−1μkxk=μk+HkTRk−1Hk+Σk−1HkTRk−1(zk−Hkμk)
令
K k = H k T R k − 1 H k T R k − 1 H k + Σ ‾ k − 1 (3) K_k=\frac{H_k^TR_k^{-1}}{H_k^TR_k^{-1}H_k+\overline\Sigma_k^{-1}}\tag{3} Kk=HkTRk−1Hk+Σk−1HkTRk−1(3)
化简 K k K_k Kk 可以得到一个更常见的形式：
K k = H k T R k − 1 Σ ‾ k H k T R k − 1 H k Σ ‾ k + I = H k T R k − 1 Σ ‾ k H k T R k − 1 H k Σ ‾ k + I = R k − 1 Σ ‾ k R k − 1 H k Σ ‾ k + ( H k T ) − 1 = Σ ‾ k H k Σ ‾ k + R k ( H k T ) − 1 = Σ ‾ k H k T H k Σ ‾ k H k T + R k K_k=\frac{H_k^TR_k^{-1}\overline\Sigma_k}{H_k^TR_k^{-1}H_k\overline\Sigma_k+I}\\ =\frac{H_k^TR_k^{-1}\overline\Sigma_k}{H_k^TR_k^{-1}H_k\overline\Sigma_k+I}\\ =\frac{R_k^{-1}\overline\Sigma_k}{R_k^{-1}H_k\overline\Sigma_k+(H_k^T)^{-1}}\\ =\frac{\overline\Sigma_k}{H_k\overline\Sigma_k+R_k(H_k^T)^{-1}}\\ =\frac{\overline\Sigma_kH_k^T}{H_k\overline\Sigma_kH_k^T+R_k} Kk=HkTRk−1HkΣk+IHkTRk−1Σk=HkTRk−1HkΣk+IHkTRk−1Σk=Rk−1HkΣk+(HkT)−1Rk−1Σk=HkΣk+Rk(HkT)−1Σk=HkΣkHkT+RkΣkHkT
则
μ k = μ ‾ k + K k ( z k − H k μ ‾ k ) (4) \mu_k=\overline\mu_k+K_k(z_k-H_k\overline\mu_k)\tag4 μk=μk+Kk(zk−Hkμk)(4)
二阶导数：
Σ k − 1 = ∂ 2 J ∂ x k 2 = H k T R k − 1 H k + Σ ‾ k − 1 \Sigma_k^{-1}=\frac{\partial^2J}{\partial x_k^2}=H_k^TR_k^{-1}H_k+\overline\Sigma_k^{-1} Σk−1=∂xk2∂2J=HkTRk−1Hk+Σk−1
将 K k K_k Kk 代入 Σ k \Sigma_k Σk 的表达式中，可以得到更加简化的表达形式
Σ k = ( I − K k H k ) Σ ‾ k (5) \Sigma_k=(I-K_kH_k)\overline\Sigma_k\tag5 Σk=(I−KkHk)Σk(5)
结论
至此，卡尔曼滤波器的五条公式全部推出，分别为式（1~5），即：
μ ‾ k = F k u k − 1 + B k u k \overline\mu_k=F_ku_{k-1}+B_ku_k μk=Fkuk−1+Bkuk
Σ ‾ k = F k Σ k − 1 F k T + Q k \overline\Sigma_k=F_k\Sigma_{k-1}F_k^T+Q_k Σk=FkΣk−1FkT+Qk
K k = Σ ‾ k H k T H k Σ ‾ k H k T + R k K_k=\frac{\overline\Sigma_kH_k^T}{H_k\overline\Sigma_kH_k^T+R_k} Kk=HkΣkHkT+RkΣkHkT
μ k = μ ‾ k + K k ( z k − H k μ ‾ k ) \mu_k=\overline\mu_k+K_k(z_k-H_k\overline\mu_k) μk=μk+Kk(zk−Hkμk)
Σ k = ( I − K k H k ) Σ ‾ k \Sigma_k=(I-K_kH_k)\overline\Sigma_k Σk=(I−KkHk)Σk
可以看到，卡尔曼滤波器即是在贝叶斯滤波器的基础上加入了带有噪声的模型。观察 K k K_k Kk 可以发现
lim R k → 0 K k = H k − 1 lim Σ ^ k → 0 K k = 0 \lim_{R_k\rightarrow0}K_k=H_k^{-1}\\ \lim_{\hat\Sigma_k\rightarrow0}K_k=0 Rk→0limKk=Hk−1Σ^k→0limKk=0
即当观测误差趋于 0 时，引入观测值进行修正的权重将增大；当预测误差趋于 0 时，引入观测值进行修正的权重将减小并趋于 0。
卡尔曼滤波器的实现
卡尔曼滤波器通过如下的算法进行实现：
![![img2][img2]](https://img-blog.csdnimg.cn/20201026152127117.png#pic_center)
接下来在倒立摆模型中实现卡尔曼滤波器，倒立摆模型预测方程近似如下：
[ θ k + 1 θ ˙ k + 1 x k + 1 x ˙ k + 1 ] = [ 1 t 0 0 0 1 0 0 0 0 1 t 0 0 0 1 ] [ θ k θ ˙ k x k x ˙ k ] + [ 0 k 0 1 − m l k M + m ] F \left[\begin{matrix} \theta_{k+1}\\\dot{\theta}_{k+1}\\x_{k+1}\\\dot{x}_{k+1} \end{matrix}\right]= \left[\begin{matrix} 1&t&0&0\\0&1&0&0\\0&0&1&t\\0&0&0&1 \end{matrix}\right] \left[\begin{matrix} \theta_{k}\\\dot{\theta}_{k}\\x_{k}\\\dot{x}_{k} \end{matrix}\right]+ \left[\begin{matrix} 0\\k\\0\\\frac{1-mlk}{M+m} \end{matrix}\right]F ⎣⎢⎢⎡θk+1θ˙k+1xk+1x˙k+1⎦⎥⎥⎤=⎣⎢⎢⎡1000t100001000t1⎦⎥⎥⎤⎣⎢⎢⎡θkθ˙kxkx˙k⎦⎥⎥⎤+⎣⎢⎢⎡0k0M+m1−mlk⎦⎥⎥⎤F
其中 x , θ , x ˙ , θ ˙ x,\theta,\dot{x},\dot{\theta} x,θ,x˙,θ˙ 为小车位移、杆角度、小车速度、杆角速度， M , m M,m M,m 为小车重量、杆重量， l l l 为杆长度， F F F 为向小车施加的力， t t t 在离散系统中为两次更新间经过的时间， k = − 1 / ( l ( 4 / 3 − m / ( m + M ) ) ( m + M ) ) k={-1}/(l(4/3-m/(m+M))(m+M)) k=−1/(l(4/3−m/(m+M))(m+M))。
在实现中一般不希望或不能观测所有的变量，而且在观测中会引入误差，假设只有小车速度和杆角速度能被测量，观测方程如下：
[ θ k + 1 x k + 1 ] = [ 0 1 0 0 0 0 0 1 ] [ θ k + 1 θ ˙ k + 1 x k + 1 x ˙ k + 1 ] \left[\begin{matrix} \theta_{k+1}\\x_{k+1} \end{matrix}\right]= \left[\begin{matrix} 0&1&0&0\\0&0&0&1 \end{matrix}\right] \left[\begin{matrix} \theta_{k+1}\\\dot{\theta}_{k+1}\\x_{k+1}\\\dot{x}_{k+1} \end{matrix}\right] [θk+1xk+1]=[00100001]⎣⎢⎢⎡θk+1θ˙k+1xk+1x˙k+1⎦⎥⎥⎤
使用卡尔曼滤波器实现状态估计。使用 Open AI 的开源库 gym 中的CartPole-v1 进行仿真，使用 PID 算法进行控制。仿真结果如下图：
卡尔曼滤波器的 python 代码如下：
class KFilter:
def __init__(self, f_mat, b_mat, q_mat, h_mat, r_mat):
self.f_mat = f_mat
self.b_mat = b_mat
self.q_mat = q_mat
self.h_mat = h_mat
self.r_mat = r_mat
def kal_filter(self, x_mat, p_mat, z_mat, action):
x_predict = self.f_mat * x_mat + self.b_mat * action
p_predict = self.f_mat * p_mat * self.f_mat.T + self.q_mat
k_num = p_predict * self.h_mat.T * np.linalg.pinv(self.h_mat * p_predict * self.h_mat.T + self.r_mat)
x_mat = x_predict + k_num * (z_mat - self.h_mat * x_predict)
p_mat = (np.eye(4) - k_num * self.h_mat) * p_predict
return x_mat, p_mat
倒立摆的 PID 算法修改自 GitHub 3
参考文献
[1] [Wikipedia] (https://zh.wikipedia.org/wiki/%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2) [2] [卡尔曼滤波器的数学推导](https://blog.csdn.net/RoboChengzi/article/details/105850925)[3] The PID control implementation of cart-pole environment in gym