前置基础#

连续时间状态空间表达式#

对于线性定常系统，连续时间状态空间表达式为：

\dot{x}(t)=A_cx(t)+B_cu(t)

y(t)=C_cx(t)+D_cu(t)

其中 $x(t)$ 为状态向量， $u(t)$ 为输入向量， $y(t)$ 为输出向量。 $A_c$ 描述状态之间的关系， $B_c$ 描述输入对状态的影响， $C_c$ 将状态转换为输出， $D_c$ 为输入到输出的直接传递矩阵。

下标 $c$ 表示连续时间，后面的下标 $d$ 表示离散时间。

离散时间状态空间表达式#

令 $x_k=x(kT)$ ， $u_k=u(kT)$ ，离散时间状态空间表达式为：

x_{k+1}=A_dx_k+B_du_k

y_k=C_dx_k+D_du_k

其中 $T$ 为采样时间， $k$ 为采样序号。下面介绍由连续时间模型求 $A_d$ 和 $B_d$ 的两种方法。

向前欧拉#

用相邻两个采样点的差分近似导数：

\dot{x}(kT)\approx\frac{x_{k+1}-x_k}{T}

代入连续时间状态方程得：

x_{k+1}=x_k+T(A_cx_k+B_cu_k)

整理得：

A_d=I+TA_c,\qquad B_d=TB_c

C_d=C_c,\qquad D_d=D_c

其中 $I$ 为与 $A_c$ 同阶的单位矩阵。向前欧拉是近似离散化，采样时间越小，通常越接近连续系统；采样时间过大时，也可能使原本稳定的系统在离散后变得不稳定。

精确离散化#

假设每个采样周期内输入保持不变，即 $u(t)=u_k$ ， $kT\leq t<(k+1)T$ 。对连续时间状态方程求解得：

x_{k+1}=e^{A_cT}x_k+ \left(\int_0^T e^{A_c\tau}B_c\,d\tau\right)u_k

因此：

A_d=e^{A_cT},\qquad B_d=\int_0^T e^{A_c\tau}B_c\,d\tau

C_d=C_c,\qquad D_d=D_c

若 $A_c$ 可逆，则还可以写成：

B_d=A_c^{-1}(A_d-I)B_c

其中 $e^{A_cT}$ 为矩阵指数。这里的“精确”以采样周期内输入不变为前提，也就是零阶保持。后面的仿真使用向前欧拉离散化。

贝叶斯法则#

贝叶斯法则为：

p(x\mid z)=\frac{p(z\mid x)p(x)}{p(z)}

其中：

$p(x)$ 是先验，表示得到当前观测之前，对 $x$ 的认识。
$p(z\mid x)$ 是似然，表示状态为 $x$ 时，得到观测 $z$ 的可能性。
$p(x\mid z)$ 是后验，表示得到观测 $z$ 后，对 $x$ 更新后的认识。
$p(z)$ 是归一化因子，使后验概率密度的积分为1。

当 $z$ 已经给定时， $p(z)$ 与 $x$ 无关，因此可以写成：

p(x\mid z)\propto p(z\mid x)p(x)

在KF中，先根据系统数学模型得到先验，再结合传感器的观测得到后验。

方差#

对于标量随机变量 $X$ ，方差为：

D(X)=E\left[(X-E(X))^2\right]

对于状态向量 $x$ ，对应的是协方差矩阵：

P=E\left[(x-E(x))(x-E(x))^T\right]

若用 $\hat{x}$ 估计 $x$ ，令估计误差为 $e=x-\hat{x}$ ，在无偏估计的条件下有：

P=E[ee^T]

也就是误差乘以误差的转置，然后取均值。协方差矩阵的对角元素是各个状态的误差方差，非对角元素表示不同状态误差之间的相关性。

一阶马尔可夫性#

一阶马尔可夫性（first-order Markov property）常被称为“无后效性”：给定当前状态后，未来的演化只依赖当前状态，而与更早的历史无关。

p(x_k\mid x_0,x_1,\ldots,x_{k-1}) =p(x_k\mid x_{k-1})

“一阶”表示记忆长度为 1：只需要记住“现在”，不需要记住“过去”。

对于包含已知输入的状态方程，还需要给定对应的输入：

p(x_k\mid x_{0:k-1},u_{0:k-1}) =p(x_k\mid x_{k-1},u_{k-1})

其中 $x_{0:k-1}$ 表示从 $x_0$ 到 $x_{k-1}$ 的全部状态。这里要求状态已经包含描述系统演化所需的信息，并且当前过程噪声与过去的状态、输入无关。

离散KF#

核心公式#

先统一符号。本文沿用后面Simulink图中的命名， $R$ 为状态方程中的过程噪声协方差， $Q$ 为量测噪声协方差。部分资料会将这两个字母反过来使用，阅读时需要先看各自的定义。

$x_{k,\mathrm{prev}}$ 和 $P_{k,\mathrm{prev}}$ 分别表示预测得到的状态和误差协方差，也就是先验。 $\bar{x}_k$ 和 $P_k$ 分别表示结合当前观测更新后的状态和误差协方差，也就是后验。

预测#

x_{k,\mathrm{prev}}=A_d\bar{x}_{k-1}+B_du_{k-1}

P_{k,\mathrm{prev}}=A_dP_{k-1}A_d^T+R

更新#

K_k=P_{k,\mathrm{prev}}C_k^T \left(C_kP_{k,\mathrm{prev}}C_k^T+Q\right)^{-1}

\bar{x}_k=x_{k,\mathrm{prev}} +K_k\left(z_k-C_kx_{k,\mathrm{prev}}\right)

P_k=(I-K_kC_k)P_{k,\mathrm{prev}}

其中 $z_k$ 是传感器采集结果， $C_k$ 是观测矩阵， $K_k$ 是卡尔曼增益， $\bar{x}_k$ 就是滤波结果。 $z_k-C_kx_{k,\mathrm{prev}}$ 称为新息，表示实际观测与预测观测之间的差。

每个采样周期先预测，再更新。更新后的 $\bar{x}_k$ 和 $P_k$ 会用于下一个周期的预测。

推导离散KF#

离散状态空间表达式：

x_k=A_dx_{k-1}+B_du_{k-1}+\xi_{k-1}\tag{1}

z_k=C_kx_k+v_k\tag{2}

其中 $\xi_{k-1}$ 为直接加在状态上的过程噪声， $v_k$ 为量测噪声。假设二者都是零均值高斯白噪声，相互独立，并且与初始状态独立：

\xi_{k-1}\sim\mathcal{N}(0,R),\qquad v_k\sim\mathcal{N}(0,Q)

初始状态服从高斯分布，均值为 $\bar{x}_0$ ，协方差为 $P_0$ 。 $u_k$ 是已知输入。为便于下面求逆，假设 $P_{k,\mathrm{prev}}$ 和 $Q$ 为正定矩阵。

后面的仿真将干扰 $w_k$ 加在输入端，此时 $\xi_k=B_dw_k$ ， $R$ 需要由输入干扰的协方差换算得到。

由系统数学模型（状态空间表达式）对 $x_k$ 进行预测#

对(1)两侧求期望。这里的期望以已经得到的历史观测和已知输入为条件，由于过程噪声均值为0，有：

x_{k,\mathrm{prev}}=A_d\bar{x}_{k-1}+B_du_{k-1}\tag{3}

由(1) - (3)得预测误差：

\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}

有了误差可以得出 $x_k$ 的预测误差协方差：

P_{k,\mathrm{prev}} =E\left[e_{k,\mathrm{prev}}e_{k,\mathrm{prev}}^T\right]\tag{5}

将(4)代入(5)得：

\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}

由于当前过程噪声与上一时刻的估计误差独立，且均值为0，中间两项为0，因此：

P_{k,\mathrm{prev}}=A_dP_{k-1}A_d^T+R\tag{6}

到这里，根据上一时刻的滤波结果，得到了当前时刻的状态预测值和预测误差协方差。

由贝叶斯法则对 $P_k$ 、 $K_k$ 、 $\bar{x}_k$ 进行更新#

计算 $x_k$ 的概率分布#

得到当前观测 $z_k$ 后，根据贝叶斯法则有：

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})}

这里的初始分布由 $\bar{x}_0$ 和 $P_0$ 给定。分母与待估计的 $x_k$ 无关，将分母拿掉得：

\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}

由一阶马尔可夫性，预测分布只需要通过上一时刻的状态分布递推：

\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(x_k\mid u_{0:k-1},z_{1:k-1}) =\mathcal{N}(x_k;x_{k,\mathrm{prev}},P_{k,\mathrm{prev}})

由(2)和量测噪声的独立性可得，给定 $x_k$ 后，当前观测与历史观测无关，因此似然为：

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)

将这两个高斯分布代入，并去掉与 $x_k$ 无关的系数：

\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}

其中：

\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(x_k)$ 最小的条件，即使概率最大的条件#

在离散卡尔曼滤波（KF）中，让后验概率密度更大（即求最大后验估计，MAP），本质上是因为我们希望在获得新的观测数据后，寻找当前状态 $x_k$ 最有可能的取值。

由于后验概率密度正比于 $\exp[-J(x_k)/2]$ ，因此使 $J(x_k)$ 最小，就是使后验概率密度最大。

对 $J(x_k)$ 求导：

\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)

令导数为0，整理得：

\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

又因为：

\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

满足上式条件时可以使 $J(x_k)$ 取得唯一的最小值。将这个 $x_k$ 记为 $\bar{x}_k$ 。

将 $J(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{常数}

与高斯分布的指数部分比较，可得后验协方差：

P_k=\left(P_{k,\mathrm{prev}}^{-1}+C_k^TQ^{-1}C_k\right)^{-1}

因此：

\bar{x}_k=P_k\left(P_{k,\mathrm{prev}}^{-1}x_{k,\mathrm{prev}} +C_k^TQ^{-1}z_k\right)

利用矩阵求逆引理，可以将 $P_k$ 改写成：

P_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}}

令卡尔曼增益为：

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}}

由上述定义还可以得到 $P_kC_k^TQ^{-1}=K_k$ 和 $P_kP_{k,\mathrm{prev}}^{-1}=I-K_kC_k$ ，代入 $\bar{x}_k$ 的表达式：

\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}

整理得更新公式：

\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}

其中 $K_k$ 用于分配预测和观测的权重。 $P_k$ 和 $\bar{x}_k$ 是根据贝叶斯法则更新后的值（后验）， $P_{k,\mathrm{prev}}$ 和 $x_{k,\mathrm{prev}}$ 是根据数学模型计算的预测值（先验）。

$\bar{x}_k$ 是滤波后的结果。在这里的线性高斯条件下，后验的最大值点也等于后验均值，因此这个结果同时也是最小均方误差估计。

Simulink仿真#

背景#

系统由两个质量块和三个弹簧—阻尼支路组成。两个质量块分别受到外力 $u_1+d_1$ 、 $u_2+d_2$ ，其中 $u_1$ 、 $u_2$ 为已知输入， $d_1$ 、 $d_2$ 为未知干扰。传感器分别测量两个质量块的位移 $p_1$ 、 $p_2$ 。

三个连接支路中的弹簧和阻尼器均为并联。位移相对无外力时的平衡位置定义，位移、速度和外力均以向右为正。忽略摩擦非线性、限位和弹簧非线性。

图中参数为：

\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}

求连续时间状态空间表达式#

根据牛顿第二定律，两个质量块分别有：

m_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_1

m_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_2

令 $v_1=\dot{p}_1$ ， $v_2=\dot{p}_2$ ，输入和干扰分别为：

u=\begin{bmatrix}u_1\\u_2\end{bmatrix},\qquad d=\begin{bmatrix}d_1\\d_2\end{bmatrix}

取 $x=[p_1,v_1,p_2,v_2]^T$ ，代入上述参数：

\dot x=A_cx+B_c(u+d),\qquad y=Cx

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}

C= \begin{bmatrix} 1&0&0&0\\ 0&0&1&0 \end{bmatrix},\qquad D=0_{2\times2}

求向前欧拉离散时间表达式#

由 $A_d=I_4+TA_c$ 、 $B_d=TB_c$ 得：

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}

将离散后的输入干扰记为 $w_k$ ，并在输出端加入量测误差 $v_k$ ：

x_{k+1}=A_dx_k+B_d(u_k+w_k)

y_k=Cx_k,\qquad z_k=y_k+v_k=Cx_k+v_k

其中 $w_k$ 和 $v_k$ 分别是输入干扰和量测误差，均为 $2\times1$ 向量。状态中的 $v_1$ 、 $v_2$ 表示速度，这里的 $v_k$ 表示量测噪声。

令输入干扰的协方差为 $W=E[w_kw_k^T]$ ，则加在状态上的过程噪声为 $\xi_k=B_dw_k$ ，因此预测公式中的 $R$ 应为：

R=E[\xi_k\xi_k^T]=B_dWB_d^T

量测噪声协方差为：

Q=E[v_kv_k^T]

$R$ 是 $4\times4$ 矩阵， $Q$ 是 $2\times2$ 矩阵。输入噪声经过 $B_d$ 后才影响状态，所以不能直接将 $2\times2$ 的 $W$ 接到预测 $P_k$ 的 $R$ 端口。

搭建原系统模型#

设置仿真步长为1e-6。

这里令采样时间 $T=10^{-6}\,\mathrm{s}$ 。原系统、KF以及噪声源均按这个周期更新，所有Unit Delay块的采样时间也设为 $T$ ，保证图中的 $1/z$ 表示一个采样周期的延迟。

求 $A_d$ 。

A_d=I_4+TA_c

求 $B_d$ 。

B_d=TB_c

求 $x_{k+1}$ 。

x_{k+1}=A_dx_k+B_du_k

这个子系统只负责状态递推，输入干扰会在外部与 $u_k$ 相加后一起送入。

求 $y_k$ 。

y_k=Cx_k

将上面4个模型分别创建子系统，然后搭建系统模型。

其中 $w_k$ 是噪声/干扰。将 $u_k+w_k$ 接入状态递推子系统，得到 $x_{k+1}$ ，再通过Unit Delay反馈，作为下一次计算时的 $x_k$ 。Unit Delay的初始值设为 $4\times1$ 的初始状态 $x_0$ ，本例取零向量。

矩阵相乘的Product块需要设置为矩阵乘法，乘法顺序与公式一致。

将第5步的系统创建为子系统。

保留 $y_k$ 、 $A_d$ 、 $B_d$ 和 $x_k$ 四个输出。 $y_k$ 用于查看实际输出， $A_d$ 和 $B_d$ 送入KF， $x_k$ 用于仿真中生成传感器观测。

搭建KF滤波器#

观测#

观测器观测 $p_1$ 、 $p_2$ 两个量。因此直接取：

C_k=C= \begin{bmatrix} 1&0&0&0\\ 0&0&1&0 \end{bmatrix}

则有：

z_k=C_kx_k+v_k =\begin{bmatrix}p_{1,k}\\p_{2,k}\end{bmatrix}+v_k

这一步用原系统的 $x_k$ 模拟传感器。实际使用时，直接将传感器采集到的 $z_k$ 送入滤波器即可。滤波器的预测和更新只使用已知输入与观测。

预测#

估计 $x_k$ 。

x_{k,\mathrm{prev}}=A_d\bar{x}_{k-1}+B_du_{k-1}

其中 $\bar{x}_{k-1}$ 是上一时刻更新后的状态， $u_{k-1}$ 是上一采样周期作用于系统的已知输入。输入端需要延迟一个周期，和状态递推的时序对应。

估计 $P_k$ 。

P_{k,\mathrm{prev}}=A_dP_{k-1}A_d^T+R

这里的 $P_{k-1}$ 也是上一时刻更新后的值。注意右侧乘的是 $A_d$ 的转置。

更新#

更新 $K_k$ 。

K_k=P_{k,\mathrm{prev}}C_k^T \left(C_kP_{k,\mathrm{prev}}C_k^T+Q\right)^{-1}

先求 $C_kP_{k,\mathrm{prev}}C_k^T+Q$ ，再求逆，最后按公式顺序相乘。图中的Inverse块对整个 $2\times2$ 矩阵求逆。

更新 $\bar{x}_k$ 。

\bar{x}_k=x_{k,\mathrm{prev}} +K_k(z_k-C_kx_{k,\mathrm{prev}})

先计算预测输出 $C_kx_{k,\mathrm{prev}}$ ，用观测 $z_k$ 减去预测输出，再乘以 $K_k$ ，最后加回预测状态。

更新 $P_k$ 。

P_k=(I_4-K_kC_k)P_{k,\mathrm{prev}}

其中 $I_4$ 是 $4\times4$ 单位矩阵，更新后的 $P_k$ 需要反馈到下一时刻的预测部分。

滤波器输出 $\bar{y}_k=C_k\bar{x}_k$ 。

\bar{y}_k=C_k\bar{x}_k =\begin{bmatrix}\bar{p}_{1,k}\\\bar{p}_{2,k}\end{bmatrix}

虽然只观测两个位移，KF估计的状态仍然有四个分量，两个速度也包含在 $\bar{x}_k$ 中。

滤波器#

前7个系统打包成子系统，然后搭建滤波器。

$\bar{x}_k$ 和 $P_k$ 分别经过一个Unit Delay，得到下一次预测所需的 $\bar{x}_{k-1}$ 和 $P_{k-1}$ 。 $u_k$ 也经过一个Unit Delay，得到 $u_{k-1}$ 。

状态反馈延时块的初值设为 $\bar{x}_0$ ，协方差反馈延时块的初值设为 $P_0$ ，输入延时块的初值可设为零向量。 $\bar{x}_0$ 表示对初始状态的估计， $P_0$ 表示这个估计的不确定程度。

前面的公式从 $k=1$ 开始递推。若模型在 $t=0$ 启动，可用Switch在首个采样点输出 $\bar{x}_0$ 和 $P_0$ ，之后再输出更新结果，使 $x_k$ 、 $z_k$ 和 $u_{k-1}$ 在时间上对应。

最后将滤波器打包成子系统，再将原系统模型和滤波器的相关端口连接起来。

原系统和滤波器使用同一个 $u_k$ ，但 $w_k$ 只加在原系统的输入端。滤波器通过 $R$ 描述未知干扰带来的不确定性，不将实际的 $w_k$ 送入预测公式。

最后整体打包，然后输入相关矩阵和参数#

$A_c$ 、 $B_c$ 和 $C_c$ 使用前面求出的矩阵。其余参数可以先取：

T=10^{-6}\,\mathrm{s},\qquad I=I_4,\qquad u_k=\begin{bmatrix}1\\2\end{bmatrix}

x_0=\bar{x}_0=0_{4\times1},\qquad P_0=I_4

下面给出一组示例噪声参数：

W=0.01I_2,\qquad Q=10^{-4}I_2

则：

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}

代入 $T=10^{-6}\,\mathrm{s}$ 后， $R$ 的第2、4个对角元素均为 $10^{-14}$ 。这里的 $W$ 描述每个采样点上离散输入干扰的方差， $R$ 已经包含了 $B_d$ 中的采样时间，不能在预测公式中再乘一次 $T$ 。

噪声可以用Random Number块生成。 $w_k$ 的均值设为[0;0]、方差设为[0.01;0.01]； $v_k$ 的均值设为[0;0]、方差设为[1e-4;1e-4]。采样时间均设为 $T$ ，各通道使用不同的随机种子，并将输出设置为 $2\times1$ 列向量。块中的Variance填写的是方差。

噪声源的实际方差要与 $W$ 、 $Q$ 对应。改变输入噪声方差或采样时间后，也需要重新计算 $R=B_dWB_d^T$ 。

运行#

建议将Scope的采样时间改为0.1s，系统仿真总时间设置为30s。这里的0.1s只用于示波器显示，原系统和KF的采样时间仍然为 $T$ 。

按图中的窗口位置，左侧的示波器是模型的实际输出 $y_k$ ，包含输入干扰对运动的影响，但没有加入传感器的量测误差。右侧的示波器是观测 $z_k$ 和滤波结果 $\bar{y}_k$ 的输出，每个信号分别包含两个位移通道。

实际上， $z_k$ 是从传感器得到的数据，其中包含量测误差。将 $z_k$ 和 $\bar{y}_k$ 与 $y_k$ 比较，可以查看滤波后是否更接近实际输出。图中的噪声大小以原仿真设置为准，使用上面的示例参数时，具体曲线会有所不同。

不考虑噪声时，恒定输入 $u=[1,2]^T$ 对应的稳态满足：

\begin{bmatrix}3&-1\\-1&3\end{bmatrix} \begin{bmatrix}p_1\\p_2\end{bmatrix} =\begin{bmatrix}1\\2\end{bmatrix}

解得 $p_1=0.625\,\mathrm{m}$ ， $p_2=0.875\,\mathrm{m}$ ，可以用来核对输出最终趋向的位置。

音乐

音乐

前置基础#

连续时间状态空间表达式#

离散时间状态空间表达式#

向前欧拉#

精确离散化#

贝叶斯法则#

方差#

一阶马尔可夫性#

离散KF#

核心公式#

预测#

更新#

推导离散KF#

由系统数学模型（状态空间表达式）对 $x_k$ 进行预测#

由贝叶斯法则对 $P_k$ 、 $K_k$ 、 $\bar{x}_k$ 进行更新#

计算 $x_k$ 的概率分布#

求使 $J(x_k)$ 最小的条件，即使概率最大的条件#

Simulink仿真#

背景#

求连续时间状态空间表达式#

求向前欧拉离散时间表达式#

搭建原系统模型#

搭建KF滤波器#

观测#

预测#

更新#

滤波器#

最后整体打包，然后输入相关矩阵和参数#

运行#

文章分享

音乐

离散KF仿真

前置基础#

连续时间状态空间表达式#

离散时间状态空间表达式#

向前欧拉#

精确离散化#

贝叶斯法则#

方差#

一阶马尔可夫性#

离散KF#

核心公式#

预测#

更新#

推导离散KF#

由系统数学模型（状态空间表达式）对xkx_kxk​进行预测#

由贝叶斯法则对PkP_kPk​、KkK_kKk​、xˉk\bar{x}_kxˉk​进行更新#

计算xkx_kxk​的概率分布#

求使J(xk)J(x_k)J(xk​)最小的条件，即使概率最大的条件#

Simulink仿真#

背景#

求连续时间状态空间表达式#

求向前欧拉离散时间表达式#

搭建原系统模型#

搭建KF滤波器#

观测#

预测#

更新#

滤波器#

最后整体打包，然后输入相关矩阵和参数#

运行#

文章分享

由系统数学模型（状态空间表达式）对 $x_k$ 进行预测#

由贝叶斯法则对 $P_k$ 、 $K_k$ 、 $\bar{x}_k$ 进行更新#

计算 $x_k$ 的概率分布#

求使 $J(x_k)$ 最小的条件，即使概率最大的条件#