前置基础#
连续时间状态空间表达式#
对于线性定常系统，连续时间状态空间表达式为：
x˙(t)=Acx(t)+Bcu(t)\dot{x}(t)=A_cx(t)+B_cu(t)x˙(t)=Acx(t)+Bcu(t)y(t)=Ccx(t)+Dcu(t)y(t)=C_cx(t)+D_cu(t)y(t)=Ccx(t)+Dcu(t)其中x(t)x(t)x(t)为状态向量，u(t)u(t)u(t)为输入向量，y(t)y(t)y(t)为输出向量。AcA_cAc描述状态之间的关系，BcB_cBc描述输入对状态的影响，CcC_cCc将状态转换为输出，DcD_cDc为输入到输出的直接传递矩阵。
下标ccc表示连续时间，后面的下标ddd表示离散时间。
离散时间状态空间表达式#
令xk=x(kT)x_k=x(kT)xk=x(kT)，uk=u(kT)u_k=u(kT)uk=u(kT)，离散时间状态空间表达式为：
xk+1=Adxk+Bdukx_{k+1}=A_dx_k+B_du_kxk+1=Adxk+Bdukyk=Cdxk+Dduky_k=C_dx_k+D_du_kyk=Cdxk+Dduk其中TTT为采样时间，kkk为采样序号。下面介绍由连续时间模型求AdA_dAd和BdB_dBd的两种方法。
向前欧拉#
用相邻两个采样点的差分近似导数：
x˙(kT)≈xk+1−xkT\dot{x}(kT)\approx\frac{x_{k+1}-x_k}{T}x˙(kT)≈Txk+1−xk代入连续时间状态方程得：
xk+1=xk+T(Acxk+Bcuk)x_{k+1}=x_k+T(A_cx_k+B_cu_k)xk+1=xk+T(Acxk+Bcuk)整理得：
Ad=I+TAc,Bd=TBcA_d=I+TA_c,\qquad B_d=TB_cAd=I+TAc,Bd=TBcCd=Cc,Dd=DcC_d=C_c,\qquad D_d=D_cCd=Cc,Dd=Dc其中III为与AcA_cAc同阶的单位矩阵。向前欧拉是近似离散化，采样时间越小，通常越接近连续系统；采样时间过大时，也可能使原本稳定的系统在离散后变得不稳定。
精确离散化#
假设每个采样周期内输入保持不变，即u(t)=uku(t)=u_ku(t)=uk，kT≤t<(k+1)TkT\leq t<(k+1)TkT≤t<(k+1)T。对连续时间状态方程求解得：
xk+1=eAcTxk+(∫0TeAcτBc dτ)ukx_{k+1}=e^{A_cT}x_k+ \left(\int_0^T e^{A_c\tau}B_c\,d\tau\right)u_kxk+1=eAcTxk+(∫0TeAcτBcdτ)uk因此：
Ad=eAcT,Bd=∫0TeAcτBc dτA_d=e^{A_cT},\qquad B_d=\int_0^T e^{A_c\tau}B_c\,d\tauAd=eAcT,Bd=∫0TeAcτBcdτCd=Cc,Dd=DcC_d=C_c,\qquad D_d=D_cCd=Cc,Dd=Dc若AcA_cAc可逆，则还可以写成：
Bd=Ac−1(Ad−I)BcB_d=A_c^{-1}(A_d-I)B_cBd=Ac−1(Ad−I)Bc其中eAcTe^{A_cT}eAcT为矩阵指数。这里的“精确”以采样周期内输入不变为前提，也就是零阶保持。后面的仿真使用向前欧拉离散化。
贝叶斯法则#
贝叶斯法则为：
p(x∣z)=p(z∣x)p(x)p(z)p(x\mid z)=\frac{p(z\mid x)p(x)}{p(z)}p(x∣z)=p(z)p(z∣x)p(x)其中：
- p(x)p(x)p(x)是先验，表示得到当前观测之前，对xxx的认识。
- p(z∣x)p(z\mid x)p(z∣x)是似然，表示状态为xxx时，得到观测zzz的可能性。
- p(x∣z)p(x\mid z)p(x∣z)是后验，表示得到观测zzz后，对xxx更新后的认识。
- p(z)p(z)p(z)是归一化因子，使后验概率密度的积分为1。
当zzz已经给定时，p(z)p(z)p(z)与xxx无关，因此可以写成：
p(x∣z)∝p(z∣x)p(x)p(x\mid z)\propto p(z\mid x)p(x)p(x∣z)∝p(z∣x)p(x)在KF中，先根据系统数学模型得到先验，再结合传感器的观测得到后验。
方差#
对于标量随机变量XXX，方差为：
D(X)=E[(X−E(X))2]D(X)=E\left[(X-E(X))^2\right]D(X)=E[(X−E(X))2]对于状态向量xxx，对应的是协方差矩阵：
P=E[(x−E(x))(x−E(x))T]P=E\left[(x-E(x))(x-E(x))^T\right]P=E[(x−E(x))(x−E(x))T]若用x^\hat{x}x^估计xxx，令估计误差为e=x−x^e=x-\hat{x}e=x−x^，在无偏估计的条件下有：
P=E[eeT]P=E[ee^T]P=E[eeT]也就是误差乘以误差的转置，然后取均值。协方差矩阵的对角元素是各个状态的误差方差，非对角元素表示不同状态误差之间的相关性。
一阶马尔可夫性#
一阶马尔可夫性（first-order Markov property）常被称为“无后效性”：给定当前状态后，未来的演化只依赖当前状态，而与更早的历史无关。
p(xk∣x0,x1,…,xk−1)=p(xk∣xk−1)p(x_k\mid x_0,x_1,\ldots,x_{k-1}) =p(x_k\mid x_{k-1})p(xk∣x0,x1,…,xk−1)=p(xk∣xk−1)“一阶”表示记忆长度为 1：只需要记住“现在”，不需要记住“过去”。
对于包含已知输入的状态方程，还需要给定对应的输入：
p(xk∣x0:k−1,u0:k−1)=p(xk∣xk−1,uk−1)p(x_k\mid x_{0:k-1},u_{0:k-1}) =p(x_k\mid x_{k-1},u_{k-1})p(xk∣x0:k−1,u0:k−1)=p(xk∣xk−1,uk−1)其中x0:k−1x_{0:k-1}x0:k−1表示从x0x_0x0到xk−1x_{k-1}xk−1的全部状态。这里要求状态已经包含描述系统演化所需的信息，并且当前过程噪声与过去的状态、输入无关。
离散KF#
核心公式#
先统一符号。本文沿用后面Simulink图中的命名，RRR为状态方程中的过程噪声协方差，QQQ为量测噪声协方差。部分资料会将这两个字母反过来使用，阅读时需要先看各自的定义。
xk,prevx_{k,\mathrm{prev}}xk,prev和Pk,prevP_{k,\mathrm{prev}}Pk,prev分别表示预测得到的状态和误差协方差，也就是先验。xˉk\bar{x}_kxˉk和PkP_kPk分别表示结合当前观测更新后的状态和误差协方差，也就是后验。
预测#
xk,prev=Adxˉk−1+Bduk−1x_{k,\mathrm{prev}}=A_d\bar{x}_{k-1}+B_du_{k-1}xk,prev=Adxˉk−1+Bduk−1Pk,prev=AdPk−1AdT+RP_{k,\mathrm{prev}}=A_dP_{k-1}A_d^T+RPk,prev=AdPk−1AdT+R更新#
Kk=Pk,prevCkT(CkPk,prevCkT+Q)−1K_k=P_{k,\mathrm{prev}}C_k^T \left(C_kP_{k,\mathrm{prev}}C_k^T+Q\right)^{-1}Kk=Pk,prevCkT(CkPk,prevCkT+Q)−1xˉk=xk,prev+Kk(zk−Ckxk,prev)\bar{x}_k=x_{k,\mathrm{prev}} +K_k\left(z_k-C_kx_{k,\mathrm{prev}}\right)xˉk=xk,prev+Kk(zk−Ckxk,prev)Pk=(I−KkCk)Pk,prevP_k=(I-K_kC_k)P_{k,\mathrm{prev}}Pk=(I−KkCk)Pk,prev其中zkz_kzk是传感器采集结果，CkC_kCk是观测矩阵，KkK_kKk是卡尔曼增益，xˉk\bar{x}_kxˉk就是滤波结果。zk−Ckxk,prevz_k-C_kx_{k,\mathrm{prev}}zk−Ckxk,prev称为新息，表示实际观测与预测观测之间的差。
每个采样周期先预测，再更新。更新后的xˉk\bar{x}_kxˉk和PkP_kPk会用于下一个周期的预测。
推导离散KF#
离散状态空间表达式：
xk=Adxk−1+Bduk−1+ξk−1(1)x_k=A_dx_{k-1}+B_du_{k-1}+\xi_{k-1}\tag{1}xk=Adxk−1+Bduk−1+ξk−1(1)zk=Ckxk+vk(2)z_k=C_kx_k+v_k\tag{2}zk=Ckxk+vk(2)其中ξk−1\xi_{k-1}ξk−1为直接加在状态上的过程噪声，vkv_kvk为量测噪声。假设二者都是零均值高斯白噪声，相互独立，并且与初始状态独立：
ξk−1∼N(0,R),vk∼N(0,Q)\xi_{k-1}\sim\mathcal{N}(0,R),\qquad v_k\sim\mathcal{N}(0,Q)ξk−1∼N(0,R),vk∼N(0,Q)初始状态服从高斯分布，均值为xˉ0\bar{x}_0xˉ0，协方差为P0P_0P0。uku_kuk是已知输入。为便于下面求逆，假设Pk,prevP_{k,\mathrm{prev}}Pk,prev和QQQ为正定矩阵。
后面的仿真将干扰wkw_kwk加在输入端，此时ξk=Bdwk\xi_k=B_dw_kξk=Bdwk，RRR需要由输入干扰的协方差换算得到。
由系统数学模型（状态空间表达式）对xkx_kxk进行预测#
对(1)两侧求期望。这里的期望以已经得到的历史观测和已知输入为条件，由于过程噪声均值为0，有：
xk,prev=Adxˉk−1+Bduk−1(3)x_{k,\mathrm{prev}}=A_d\bar{x}_{k-1}+B_du_{k-1}\tag{3}xk,prev=Adxˉk−1+Bduk−1(3)由(1) - (3)得预测误差：
ek,prev=xk−xk,prev=Ad(xk−1−xˉk−1)+ξk−1=Adek−1+ξk−1(4)\begin{aligned} e_{k,\mathrm{prev}} &=x_k-x_{k,\mathrm{prev}}\\ &=A_d(x_{k-1}-\bar{x}_{k-1})+\xi_{k-1}\\ &=A_de_{k-1}+\xi_{k-1} \end{aligned}\tag{4}ek,prev=xk−xk,prev=Ad(xk−1−xˉk−1)+ξk−1=Adek−1+ξk−1(4)有了误差可以得出xkx_kxk的预测误差协方差：
Pk,prev=E[ek,prevek,prevT](5)P_{k,\mathrm{prev}} =E\left[e_{k,\mathrm{prev}}e_{k,\mathrm{prev}}^T\right]\tag{5}Pk,prev=E[ek,prevek,prevT](5)将(4)代入(5)得：
Pk,prev=E[(Adek−1+ξk−1)(Adek−1+ξk−1)T]=AdE[ek−1ek−1T]AdT+AdE[ek−1ξk−1T]+E[ξk−1ek−1T]AdT+E[ξk−1ξk−1T]\begin{aligned} P_{k,\mathrm{prev}} &=E\left[(A_de_{k-1}+\xi_{k-1}) (A_de_{k-1}+\xi_{k-1})^T\right]\\ &=A_dE[e_{k-1}e_{k-1}^T]A_d^T +A_dE[e_{k-1}\xi_{k-1}^T]\\ &\quad+E[\xi_{k-1}e_{k-1}^T]A_d^T +E[\xi_{k-1}\xi_{k-1}^T] \end{aligned}Pk,prev=E[(Adek−1+ξk−1)(Adek−1+ξk−1)T]=AdE[ek−1ek−1T]AdT+AdE[ek−1ξk−1T]+E[ξk−1ek−1T]AdT+E[ξk−1ξk−1T]由于当前过程噪声与上一时刻的估计误差独立，且均值为0，中间两项为0，因此：
Pk,prev=AdPk−1AdT+R(6)P_{k,\mathrm{prev}}=A_dP_{k-1}A_d^T+R\tag{6}Pk,prev=AdPk−1AdT+R(6)到这里，根据上一时刻的滤波结果，得到了当前时刻的状态预测值和预测误差协方差。
由贝叶斯法则对PkP_kPk、KkK_kKk、xˉk\bar{x}_kxˉk进行更新#
计算xkx_kxk的概率分布#
得到当前观测zkz_kzk后，根据贝叶斯法则有：
p(xk∣u0:k−1,z1:k)=p(zk∣xk,u0:k−1,z1:k−1)p(xk∣u0:k−1,z1:k−1)p(zk∣u0:k−1,z1:k−1)p(x_k\mid u_{0:k-1},z_{1:k}) =\frac{ p(z_k\mid x_k,u_{0:k-1},z_{1:k-1}) p(x_k\mid u_{0:k-1},z_{1:k-1}) }{p(z_k\mid u_{0:k-1},z_{1:k-1})}p(xk∣u0:k−1,z1:k)=p(zk∣u0:k−1,z1:k−1)p(zk∣xk,u0:k−1,z1:k−1)p(xk∣u0:k−1,z1:k−1)这里的初始分布由xˉ0\bar{x}_0xˉ0和P0P_0P0给定。分母与待估计的xkx_kxk无关，将分母拿掉得：
p(xk∣u0:k−1,z1:k)∝p(zk∣xk,u0:k−1,z1:k−1)⋅p(xk∣u0:k−1,z1:k−1)\begin{aligned} p(x_k\mid u_{0:k-1},z_{1:k}) \propto{}&p(z_k\mid x_k,u_{0:k-1},z_{1:k-1})\\ &\cdot p(x_k\mid u_{0:k-1},z_{1:k-1}) \end{aligned}p(xk∣u0:k−1,z1:k)∝p(zk∣xk,u0:k−1,z1:k−1)⋅p(xk∣u0:k−1,z1:k−1)由一阶马尔可夫性，预测分布只需要通过上一时刻的状态分布递推：
p(xk∣u0:k−1,z1:k−1)=∫p(xk∣xk−1,uk−1)⋅p(xk−1∣u0:k−2,z1:k−1) dxk−1\begin{aligned} p(x_k\mid u_{0:k-1},z_{1:k-1}) =\int{}&p(x_k\mid x_{k-1},u_{k-1})\\ &\cdot p(x_{k-1}\mid u_{0:k-2},z_{1:k-1})\,dx_{k-1} \end{aligned}p(xk∣u0:k−1,z1:k−1)=∫p(xk∣xk−1,uk−1)⋅p(xk−1∣u0:k−2,z1:k−1)dxk−1上一时刻的后验为高斯分布，状态方程是线性的，过程噪声也是高斯分布，因此预测分布仍然是高斯分布，其均值和协方差就是前面求出的结果：
p(xk∣u0:k−1,z1:k−1)=N(xk;xk,prev,Pk,prev)p(x_k\mid u_{0:k-1},z_{1:k-1}) =\mathcal{N}(x_k;x_{k,\mathrm{prev}},P_{k,\mathrm{prev}})p(xk∣u0:k−1,z1:k−1)=N(xk;xk,prev,Pk,prev)由(2)和量测噪声的独立性可得，给定xkx_kxk后，当前观测与历史观测无关，因此似然为：
p(zk∣xk,u0:k−1,z1:k−1)=p(zk∣xk)=N(zk;Ckxk,Q)p(z_k\mid x_k,u_{0:k-1},z_{1:k-1}) =p(z_k\mid x_k)=\mathcal{N}(z_k;C_kx_k,Q)p(zk∣xk,u0:k−1,z1:k−1)=p(zk∣xk)=N(zk;Ckxk,Q)将这两个高斯分布代入，并去掉与xkx_kxk无关的系数：
p(xk∣u0:k−1,z1:k)∝p(zk∣xk)p(xk∣u0:k−1,z1:k−1)∝exp[−12(zk−Ckxk)TQ−1(zk−Ckxk)]⋅exp[−12(xk−xk,prev)TPk,prev−1(xk−xk,prev)]∝exp[−12J(xk)]\begin{aligned} p(x_k\mid u_{0:k-1},z_{1:k}) &\propto p(z_k\mid x_k) p(x_k\mid u_{0:k-1},z_{1:k-1})\\ &\propto \exp\left[-\frac12(z_k-C_kx_k)^TQ^{-1}(z_k-C_kx_k)\right]\\ &\quad\cdot\exp\left[-\frac12(x_k-x_{k,\mathrm{prev}})^T P_{k,\mathrm{prev}}^{-1}(x_k-x_{k,\mathrm{prev}})\right]\\ &\propto \exp\left[-\frac12J(x_k)\right] \end{aligned}p(xk∣u0:k−1,z1:k)∝p(zk∣xk)p(xk∣u0:k−1,z1:k−1)∝exp[−21(zk−Ckxk)TQ−1(zk−Ckxk)]⋅exp[−21(xk−xk,prev)TPk,prev−1(xk−xk,prev)]∝exp[−21J(xk)]其中：
J(xk)=(xk−xk,prev)TPk,prev−1(xk−xk,prev)+(zk−Ckxk)TQ−1(zk−Ckxk)\begin{aligned} J(x_k) &=(x_k-x_{k,\mathrm{prev}})^TP_{k,\mathrm{prev}}^{-1} (x_k-x_{k,\mathrm{prev}})\\ &\quad+(z_k-C_kx_k)^TQ^{-1}(z_k-C_kx_k) \end{aligned}J(xk)=(xk−xk,prev)TPk,prev−1(xk−xk,prev)+(zk−Ckxk)TQ−1(zk−Ckxk)第一项描述当前状态与模型预测之间的差，第二项描述当前状态对应的输出与实际观测之间的差。协方差的逆决定这两项各自的权重。
求使J(xk)J(x_k)J(xk)最小的条件，即使概率最大的条件#
在离散卡尔曼滤波（KF）中，让后验概率密度更大（即求最大后验估计，MAP），本质上是因为我们希望在获得新的观测数据后，寻找当前状态xkx_kxk最有可能的取值。
由于后验概率密度正比于exp[−J(xk)/2]\exp[-J(x_k)/2]exp[−J(xk)/2]，因此使J(xk)J(x_k)J(xk)最小，就是使后验概率密度最大。
对J(xk)J(x_k)J(xk)求导：
∂J∂xk=2Pk,prev−1(xk−xk,prev)−2CkTQ−1(zk−Ckxk)\frac{\partial J}{\partial x_k} =2P_{k,\mathrm{prev}}^{-1}(x_k-x_{k,\mathrm{prev}}) -2C_k^TQ^{-1}(z_k-C_kx_k)∂xk∂J=2Pk,prev−1(xk−xk,prev)−2CkTQ−1(zk−Ckxk)令导数为0，整理得：
(Pk,prev−1+CkTQ−1Ck)xk=Pk,prev−1xk,prev+CkTQ−1zk\left(P_{k,\mathrm{prev}}^{-1}+C_k^TQ^{-1}C_k\right)x_k =P_{k,\mathrm{prev}}^{-1}x_{k,\mathrm{prev}}+C_k^TQ^{-1}z_k(Pk,prev−1+CkTQ−1Ck)xk=Pk,prev−1xk,prev+CkTQ−1zk又因为：
∂2J∂xk∂xkT=2(Pk,prev−1+CkTQ−1Ck)>0\frac{\partial^2J}{\partial x_k\partial x_k^T} =2\left(P_{k,\mathrm{prev}}^{-1}+C_k^TQ^{-1}C_k\right)>0∂xk∂xkT∂2J=2(Pk,prev−1+CkTQ−1Ck)>0满足上式条件时可以使J(xk)J(x_k)J(xk)取得唯一的最小值。将这个xkx_kxk记为xˉk\bar{x}_kxˉk。
将J(xk)J(x_k)J(xk)配方得：
J(xk)=(xk−xˉk)T(Pk,prev−1+CkTQ−1Ck)(xk−xˉk)+常数J(x_k)=(x_k-\bar{x}_k)^T \left(P_{k,\mathrm{prev}}^{-1}+C_k^TQ^{-1}C_k\right) (x_k-\bar{x}_k)+\text{常数}J(xk)=(xk−xˉk)T(Pk,prev−1+CkTQ−1Ck)(xk−xˉk)+常数与高斯分布的指数部分比较，可得后验协方差：
Pk=(Pk,prev−1+CkTQ−1Ck)−1P_k=\left(P_{k,\mathrm{prev}}^{-1}+C_k^TQ^{-1}C_k\right)^{-1}Pk=(Pk,prev−1+CkTQ−1Ck)−1因此：
xˉk=Pk(Pk,prev−1xk,prev+CkTQ−1zk)\bar{x}_k=P_k\left(P_{k,\mathrm{prev}}^{-1}x_{k,\mathrm{prev}} +C_k^TQ^{-1}z_k\right)xˉk=Pk(Pk,prev−1xk,prev+CkTQ−1zk)利用矩阵求逆引理，可以将PkP_kPk改写成：
Pk=Pk,prev−Pk,prevCkT(CkPk,prevCkT+Q)−1CkPk,prevP_k=P_{k,\mathrm{prev}}-P_{k,\mathrm{prev}}C_k^T \left(C_kP_{k,\mathrm{prev}}C_k^T+Q\right)^{-1} C_kP_{k,\mathrm{prev}}Pk=Pk,prev−Pk,prevCkT(CkPk,prevCkT+Q)−1CkPk,prev令卡尔曼增益为：
Kk=Pk,prevCkT(CkPk,prevCkT+Q)−1K_k=P_{k,\mathrm{prev}}C_k^T \left(C_kP_{k,\mathrm{prev}}C_k^T+Q\right)^{-1}Kk=Pk,prevCkT(CkPk,prevCkT+Q)−1则有：
Pk=(I−KkCk)Pk,prevP_k=(I-K_kC_k)P_{k,\mathrm{prev}}Pk=(I−KkCk)Pk,prev由上述定义还可以得到PkCkTQ−1=KkP_kC_k^TQ^{-1}=K_kPkCkTQ−1=Kk和PkPk,prev−1=I−KkCkP_kP_{k,\mathrm{prev}}^{-1}=I-K_kC_kPkPk,prev−1=I−KkCk，代入xˉk\bar{x}_kxˉk的表达式：
xˉk=(I−KkCk)xk,prev+Kkzk=xk,prev+Kk(zk−Ckxk,prev)\begin{aligned} \bar{x}_k &=(I-K_kC_k)x_{k,\mathrm{prev}}+K_kz_k\\ &=x_{k,\mathrm{prev}}+K_k(z_k-C_kx_{k,\mathrm{prev}}) \end{aligned}xˉk=(I−KkCk)xk,prev+Kkzk=xk,prev+Kk(zk−Ckxk,prev)整理得更新公式：
Kk=Pk,prevCkT(CkPk,prevCkT+Q)−1Pk=(I−KkCk)Pk,prevxˉk=xk,prev+Kk(zk−Ckxk,prev)\begin{aligned} K_k&=P_{k,\mathrm{prev}}C_k^T \left(C_kP_{k,\mathrm{prev}}C_k^T+Q\right)^{-1}\\ P_k&=(I-K_kC_k)P_{k,\mathrm{prev}}\\ \bar{x}_k&=x_{k,\mathrm{prev}} +K_k(z_k-C_kx_{k,\mathrm{prev}}) \end{aligned}KkPkxˉk=Pk,prevCkT(CkPk,prevCkT+Q)−1=(I−KkCk)Pk,prev=xk,prev+Kk(zk−Ckxk,prev)其中KkK_kKk用于分配预测和观测的权重。PkP_kPk和xˉk\bar{x}_kxˉk是根据贝叶斯法则更新后的值（后验），Pk,prevP_{k,\mathrm{prev}}Pk,prev和xk,prevx_{k,\mathrm{prev}}xk,prev是根据数学模型计算的预测值（先验）。
xˉk\bar{x}_kxˉk是滤波后的结果。在这里的线性高斯条件下，后验的最大值点也等于后验均值，因此这个结果同时也是最小均方误差估计。
Simulink仿真#
背景#
系统由两个质量块和三个弹簧—阻尼支路组成。两个质量块分别受到外力u1+d1u_1+d_1u1+d1、u2+d2u_2+d_2u2+d2，其中u1u_1u1、u2u_2u2为已知输入，d1d_1d1、d2d_2d2为未知干扰。传感器分别测量两个质量块的位移p1p_1p1、p2p_2p2。
三个连接支路中的弹簧和阻尼器均为并联。位移相对无外力时的平衡位置定义，位移、速度和外力均以向右为正。忽略摩擦非线性、限位和弹簧非线性。
图中参数为：
m1=m2=1 kgk1=k2=2 N/m,kc=1 N/mc1=c2=0.2 N⋅s/m,cc=0.1 N⋅s/m\begin{aligned} m_1=m_2&=1\ \mathrm{kg}\\ k_1=k_2&=2\ \mathrm{N/m},\qquad k_c=1\ \mathrm{N/m}\\ c_1=c_2&=0.2\ \mathrm{N\cdot s/m},\qquad c_c=0.1\ \mathrm{N\cdot s/m} \end{aligned}m1=m2k1=k2c1=c2=1 kg=2 N/m,kc=1 N/m=0.2 N⋅s/m,cc=0.1 N⋅s/m求连续时间状态空间表达式#
根据牛顿第二定律，两个质量块分别有：
m1p¨1=−k1p1−c1p˙1+kc(p2−p1)+cc(p˙2−p˙1)+u1+d1m_1\ddot{p}_1=-k_1p_1-c_1\dot{p}_1 +k_c(p_2-p_1)+c_c(\dot{p}_2-\dot{p}_1)+u_1+d_1m1p¨1=−k1p1−c1p˙1+kc(p2−p1)+cc(p˙2−p˙1)+u1+d1m2p¨2=−k2p2−c2p˙2+kc(p1−p2)+cc(p˙1−p˙2)+u2+d2m_2\ddot{p}_2=-k_2p_2-c_2\dot{p}_2 +k_c(p_1-p_2)+c_c(\dot{p}_1-\dot{p}_2)+u_2+d_2m2p¨2=−k2p2−c2p˙2+kc(p1−p2)+cc(p˙1−p˙2)+u2+d2令v1=p˙1v_1=\dot{p}_1v1=p˙1，v2=p˙2v_2=\dot{p}_2v2=p˙2，输入和干扰分别为：
u=[u1u2],d=[d1d2]u=\begin{bmatrix}u_1\\u_2\end{bmatrix},\qquad d=\begin{bmatrix}d_1\\d_2\end{bmatrix}u=[u1u2],d=[d1d2]取 x=[p1,v1,p2,v2]Tx=[p_1,v_1,p_2,v_2]^Tx=[p1,v1,p2,v2]T，代入上述参数：
x˙=Acx+Bc(u+d),y=Cx\dot x=A_cx+B_c(u+d),\qquad y=Cxx˙=Acx+Bc(u+d),y=CxAc=[0100−3−0.310.1000110.1−3−0.3],Bc=[00100001]A_c= \begin{bmatrix} 0&1&0&0\\ -3&-0.3&1&0.1\\ 0&0&0&1\\ 1&0.1&-3&-0.3 \end{bmatrix},\qquad B_c= \begin{bmatrix} 0&0\\ 1&0\\ 0&0\\ 0&1 \end{bmatrix}Ac=0−3011−0.300.1010−300.11−0.3,Bc=01000001C=[10000010],D=02×2C= \begin{bmatrix} 1&0&0&0\\ 0&0&1&0 \end{bmatrix},\qquad D=0_{2\times2}C=[10000100],D=02×2求向前欧拉离散时间表达式#
由Ad=I4+TAcA_d=I_4+TA_cAd=I4+TAc、Bd=TBcB_d=TB_cBd=TBc得：
Ad=[1T00−3T1−0.3TT0.1T001TT0.1T−3T1−0.3T],Bd=[00T0000T]A_d= \begin{bmatrix} 1&T&0&0\\ -3T&1-0.3T&T&0.1T\\ 0&0&1&T\\ T&0.1T&-3T&1-0.3T \end{bmatrix},\qquad B_d= \begin{bmatrix} 0&0\\ T&0\\ 0&0\\ 0&T \end{bmatrix}Ad=1−3T0TT1−0.3T00.1T0T1−3T00.1TT1−0.3T,Bd=0T00000T将离散后的输入干扰记为wkw_kwk，并在输出端加入量测误差vkv_kvk：
xk+1=Adxk+Bd(uk+wk)x_{k+1}=A_dx_k+B_d(u_k+w_k)xk+1=Adxk+Bd(uk+wk)yk=Cxk,zk=yk+vk=Cxk+vky_k=Cx_k,\qquad z_k=y_k+v_k=Cx_k+v_kyk=Cxk,zk=yk+vk=Cxk+vk其中wkw_kwk和vkv_kvk分别是输入干扰和量测误差，均为2×12\times12×1向量。状态中的v1v_1v1、v2v_2v2表示速度，这里的vkv_kvk表示量测噪声。
令输入干扰的协方差为W=E[wkwkT]W=E[w_kw_k^T]W=E[wkwkT]，则加在状态上的过程噪声为ξk=Bdwk\xi_k=B_dw_kξk=Bdwk，因此预测公式中的RRR应为：
R=E[ξkξkT]=BdWBdTR=E[\xi_k\xi_k^T]=B_dWB_d^TR=E[ξkξkT]=BdWBdT量测噪声协方差为：
Q=E[vkvkT]Q=E[v_kv_k^T]Q=E[vkvkT]RRR是4×44\times44×4矩阵，QQQ是2×22\times22×2矩阵。输入噪声经过BdB_dBd后才影响状态，所以不能直接将2×22\times22×2的WWW接到预测PkP_kPk的RRR端口。
搭建原系统模型#
- 设置仿真步长为
1e-6。
这里令采样时间T=10−6 sT=10^{-6}\,\mathrm{s}T=10−6s。原系统、KF以及噪声源均按这个周期更新，所有Unit Delay块的采样时间也设为TTT，保证图中的1/z1/z1/z表示一个采样周期的延迟。
- 求AdA_dAd。
- 求BdB_dBd。
- 求xk+1x_{k+1}xk+1。
这个子系统只负责状态递推，输入干扰会在外部与uku_kuk相加后一起送入。
- 求yky_kyk。
- 将上面4个模型分别创建子系统，然后搭建系统模型。
其中wkw_kwk是噪声/干扰。将uk+wku_k+w_kuk+wk接入状态递推子系统，得到xk+1x_{k+1}xk+1，再通过Unit Delay反馈，作为下一次计算时的xkx_kxk。Unit Delay的初始值设为4×14\times14×1的初始状态x0x_0x0，本例取零向量。
矩阵相乘的Product块需要设置为矩阵乘法，乘法顺序与公式一致。
- 将第5步的系统创建为子系统。
保留yky_kyk、AdA_dAd、BdB_dBd和xkx_kxk四个输出。yky_kyk用于查看实际输出，AdA_dAd和BdB_dBd送入KF，xkx_kxk用于仿真中生成传感器观测。
搭建KF滤波器#
观测#
- 观测器观测p1p_1p1、p2p_2p2两个量。因此直接取：
则有：
zk=Ckxk+vk=[p1,kp2,k]+vkz_k=C_kx_k+v_k =\begin{bmatrix}p_{1,k}\\p_{2,k}\end{bmatrix}+v_kzk=Ckxk+vk=[p1,kp2,k]+vk
这一步用原系统的xkx_kxk模拟传感器。实际使用时，直接将传感器采集到的zkz_kzk送入滤波器即可。滤波器的预测和更新只使用已知输入与观测。
预测#
- 估计xkx_kxk。
其中xˉk−1\bar{x}_{k-1}xˉk−1是上一时刻更新后的状态，uk−1u_{k-1}uk−1是上一采样周期作用于系统的已知输入。输入端需要延迟一个周期，和状态递推的时序对应。
- 估计PkP_kPk。
这里的Pk−1P_{k-1}Pk−1也是上一时刻更新后的值。注意右侧乘的是AdA_dAd的转置。
更新#
- 更新KkK_kKk。
先求CkPk,prevCkT+QC_kP_{k,\mathrm{prev}}C_k^T+QCkPk,prevCkT+Q，再求逆，最后按公式顺序相乘。图中的Inverse块对整个2×22\times22×2矩阵求逆。
- 更新xˉk\bar{x}_kxˉk。
先计算预测输出Ckxk,prevC_kx_{k,\mathrm{prev}}Ckxk,prev，用观测zkz_kzk减去预测输出，再乘以KkK_kKk，最后加回预测状态。
- 更新PkP_kPk。
其中I4I_4I4是4×44\times44×4单位矩阵，更新后的PkP_kPk需要反馈到下一时刻的预测部分。
- 滤波器输出yˉk=Ckxˉk\bar{y}_k=C_k\bar{x}_kyˉk=Ckxˉk。
虽然只观测两个位移，KF估计的状态仍然有四个分量，两个速度也包含在xˉk\bar{x}_kxˉk中。
滤波器#
前7个系统打包成子系统，然后搭建滤波器。
xˉk\bar{x}_kxˉk和PkP_kPk分别经过一个Unit Delay，得到下一次预测所需的xˉk−1\bar{x}_{k-1}xˉk−1和Pk−1P_{k-1}Pk−1。uku_kuk也经过一个Unit Delay，得到uk−1u_{k-1}uk−1。
状态反馈延时块的初值设为xˉ0\bar{x}_0xˉ0，协方差反馈延时块的初值设为P0P_0P0，输入延时块的初值可设为零向量。xˉ0\bar{x}_0xˉ0表示对初始状态的估计，P0P_0P0表示这个估计的不确定程度。
前面的公式从k=1k=1k=1开始递推。若模型在t=0t=0t=0启动，可用Switch在首个采样点输出xˉ0\bar{x}_0xˉ0和P0P_0P0，之后再输出更新结果，使xkx_kxk、zkz_kzk和uk−1u_{k-1}uk−1在时间上对应。
最后将滤波器打包成子系统，再将原系统模型和滤波器的相关端口连接起来。
原系统和滤波器使用同一个uku_kuk，但wkw_kwk只加在原系统的输入端。滤波器通过RRR描述未知干扰带来的不确定性，不将实际的wkw_kwk送入预测公式。
最后整体打包，然后输入相关矩阵和参数#
AcA_cAc、BcB_cBc和CcC_cCc使用前面求出的矩阵。其余参数可以先取：
T=10−6 s,I=I4,uk=[12]T=10^{-6}\,\mathrm{s},\qquad I=I_4,\qquad u_k=\begin{bmatrix}1\\2\end{bmatrix}T=10−6s,I=I4,uk=[12]x0=xˉ0=04×1,P0=I4x_0=\bar{x}_0=0_{4\times1},\qquad P_0=I_4x0=xˉ0=04×1,P0=I4下面给出一组示例噪声参数：
W=0.01I2,Q=10−4I2W=0.01I_2,\qquad Q=10^{-4}I_2W=0.01I2,Q=10−4I2则：
R=BdWBdT=0.01T2[0000010000000001]R=B_dWB_d^T =0.01T^2 \begin{bmatrix} 0&0&0&0\\ 0&1&0&0\\ 0&0&0&0\\ 0&0&0&1 \end{bmatrix}R=BdWBdT=0.01T20000010000000001代入T=10−6 sT=10^{-6}\,\mathrm{s}T=10−6s后，RRR的第2、4个对角元素均为10−1410^{-14}10−14。这里的WWW描述每个采样点上离散输入干扰的方差，RRR已经包含了BdB_dBd中的采样时间，不能在预测公式中再乘一次TTT。
噪声可以用Random Number块生成。wkw_kwk的均值设为[0;0]、方差设为[0.01;0.01]；vkv_kvk的均值设为[0;0]、方差设为[1e-4;1e-4]。采样时间均设为TTT，各通道使用不同的随机种子，并将输出设置为2×12\times12×1列向量。块中的Variance填写的是方差。
噪声源的实际方差要与WWW、QQQ对应。改变输入噪声方差或采样时间后，也需要重新计算R=BdWBdTR=B_dWB_d^TR=BdWBdT。
运行#
建议将Scope的采样时间改为0.1s，系统仿真总时间设置为30s。这里的0.1s只用于示波器显示，原系统和KF的采样时间仍然为TTT。
按图中的窗口位置，左侧的示波器是模型的实际输出yky_kyk，包含输入干扰对运动的影响，但没有加入传感器的量测误差。右侧的示波器是观测zkz_kzk和滤波结果yˉk\bar{y}_kyˉk的输出，每个信号分别包含两个位移通道。
实际上，zkz_kzk是从传感器得到的数据，其中包含量测误差。将zkz_kzk和yˉk\bar{y}_kyˉk与yky_kyk比较，可以查看滤波后是否更接近实际输出。图中的噪声大小以原仿真设置为准，使用上面的示例参数时，具体曲线会有所不同。
不考虑噪声时，恒定输入u=[1,2]Tu=[1,2]^Tu=[1,2]T对应的稳态满足：
[3−1−13][p1p2]=[12]\begin{bmatrix}3&-1\\-1&3\end{bmatrix} \begin{bmatrix}p_1\\p_2\end{bmatrix} =\begin{bmatrix}1\\2\end{bmatrix}[3−1−13][p1p2]=[12]解得p1=0.625 mp_1=0.625\,\mathrm{m}p1=0.625m，p2=0.875 mp_2=0.875\,\mathrm{m}p2=0.875m，可以用来核对输出最终趋向的位置。
