Kalman Filter 算法推导--根据B站Dr.can视频整理

Kalman Filter

• 前提：噪声信号满足正态分布

一、引例1：测量和估计

采样$k$次(测量值)，$x_1,x_2,x_3,\cdots ,x_k$,则其估计值${\hat{x}}_k$为：

${\hat{x}}_k=\frac{1}{k}(x_1+x_2+x_3+\cdots +x_k)$,变形得：

${\hat{x}}_k=\frac{1}{k}\frac{k-1}{k-1}(x_1+x_2+x_3+\cdots +x_{k-1})+\frac{1}{k}{x}_k$

$=\frac{k-1}{k}{\hat{x}}_{k-1}+\frac{1}{k}{x}_k$

$={\hat{x}}_{k-1}+\frac{1}{k}(x_k-{\hat{x}}_{k-1})$

令$\frac{1}{k}=K_k$($K_k:$Kalman增益/系数),则上式变为：

${\hat{x}}_k={\hat{x}}_{k-1}+K_k(x_k-{\hat{x}}_{k-1})$

从上述的引例可以得知，对于一个对象的估计值应该是和测量值、前次的估计值有关。

• $K_k=\frac{e_{ES{T}_{k-1}}}{e_{ES{T}_{k-1}}+e_{MEA}}$（$e_{EST}:$估计误差；$e_{MEA}:$测量误差）

二、引例2：数据融合

\quad 假设用两把尺子测量一个物体的长度，测量的结果分别为$l_1=30$，$l_2=32$，其中尺子1的标准差${\sigma }_1=2$，尺子2的标准差${\sigma }_2=4$。因此，通过这两个测量的数据如何去估计该物体的真实长度？根据kalman filter 公式，估计值与测量值之间的关系为：

\quad $\hat{l}=l_1+K_k(l_2-l_1)$

则 $\hat{l}$的方差为：

\quad $var(\hat{l})=var(l_1+K_k(l_2-l_1))$

\quad $⟹=var((1-K_k)l_1+K_k{l}_2)$

由于两把尺子的测量结果不会相互影响，因此测量结果$l_1$、$l_2$是相互独立，则上述估计值方差公式可以展开为：

\quad $var(\hat{l})=(1-K_k{)}^{2}var(l_1)+K_k^{2}var(l_2)$

要使估计值更接近于真实值，则其方差需要越小，亦即可以将问题转换为$K_k$取何值时，估计值方差最小？上式对$K_k$求导并令其等于零可得:

\quad $\frac{\partial var(\hat{l})}{\partial K_k}=-2(1-K_k)var(l_1)+2K_{k}var(l_2)=0$

\quad $⟹K_k=\frac{var(l_1)}{var(l_1)+var(l_2)}$

将$var(l_1)={\sigma }_1^2=4、var(l_2)={\sigma }_2^2=16$代入上式可得：

\quad $K_k=\frac{4}{4+16}=0.2$

将$K_k$代入$var(\hat{l})、\hat{l}$中可得方差和估计值为：

$var(\hat{l})=(1-0.2{)}^2\ast 2^2+0.2^2\ast 4^2=3.2$

\quad $\hat{l}=30+0.2(32-30)=30.4$

进一步可得估计值标准差${\sigma }_{\hat{l}}=\sqrt{3.2}\approx 1.79$

import numpy as np #
import matplotlib.pyplot as plt # 

x = np.arange(20,45,0.1) 
#设定 y 轴，载入刚才的正态分布函数
y1 = np.exp(-((x - 30)**2)/(2*2**2)) / (2 * np.sqrt(2*np.pi))#normfun(x, 30, 2)
y2 = np.exp(-((x - 32)**2)/(2*4**2)) / (2 * np.sqrt(2*np.pi))#normfun(x, 32, 4)
y3 = np.exp(-((x - 30.4)**2)/(2*np.sqrt(3.2)**2)) / (2 * np.sqrt(2*np.pi))#normfun(x, 30.4, 3.2)

plt.plot(x,y1,x,y2,x,y3)

执行上段代码可得：

三、协方差矩阵

1、协方差矩阵定义

\quad 协方差矩阵将方差和协方差关联在一起。假设对一个对象从三个维度$\text{X 、Y、 Z}$进行描述，$\text{X 、Y、 Z}$都是$n\times 1$维向量,如下：

\quad $X=\left[\begin{array}{c}{x}_1\\ x_2\\ ⋮\\ x_n\end{array}\right]Y=\left[\begin{array}{c}{y}_1\\ y_2\\ ⋮\\ y_n\end{array}\right]Z=\left[\begin{array}{c}{z}_1\\ z_2\\ ⋮\\ z_n\end{array}\right]$

$\text{X 、Y、 Z}$方差分别为${\sigma }_x^2$、${\sigma }_y^2$、${\sigma }_z^2$，$\text{X 、Y、 Z}$的均值分别为$\overline{x}$、$\overline{y}$、$\overline{z}$。则不同纬度之间的协方差可以写成：

\quad ${\sigma }_x{\sigma }_y=\frac{1}{n}[(x_1-\overline{x})(y_1-\overline{y})+(x_2-\overline{x})(y_2-\overline{y})+\cdots +(x_n-\overline{x})(y_n-\overline{y})]={\sigma }_y{\sigma }_x$

\quad ${\sigma }_x{\sigma }_z=\frac{1}{n}[(x_1-\overline{x})(z_1-\overline{z})+(x_2-\overline{x})(z_2-\overline{z})+\cdots +(x_n-\overline{x})(z_n-\overline{z})]={\sigma }_z{\sigma }_x$

\quad ${\sigma }_y{\sigma }_z=\frac{1}{n}[(y_1-\overline{y})(z_1-\overline{z})+(y_2-\overline{y})(z_2-\overline{z})+\cdots +(y_n-\overline{y})(z_n-\overline{z})]={\sigma }_z{\sigma }_y$

因此，协方差矩阵可以写成：

\quad $M_{cov}=\left[\begin{array}{ccc}{\sigma }_x^2& {\sigma }_x{\sigma }_y& {\sigma }_x{\sigma }_z\\ {\sigma }_y{\sigma }_x& {\sigma }_y^2& {\sigma }_y{\sigma }_z\\ {\sigma }_z{\sigma }_x& {\sigma }_z{\sigma }_y& {\sigma }_z^2\end{array}\right]$

2、协方差矩阵性质

• 根据以上所述可以得出协方差矩阵的一个性质：$M_{cov}=M_{cov}^T$
• 如果信号的期望为0，则协方差矩阵等于信号与其转置乘积的期望，简单例证如下：
  \qquad 假设$w$为二维向量$w=\left[\begin{array}{c}{w}_1\\ w_2\end{array}\right]$，根据协方差定义，$m_{cov}(w_i,w_j)=E[(w_i-E[w_i])(w_j-E[w_j])]$，由于$w$的期望为0，所以$E[w_i]、E[w_j]$等于零，则$m_{cov}(w_i,w_j)=E[w_i\cdot w_j]$。
  \qquad 根据相关定义，$E[w_1^2]=VAR(w_1)+E[w_1^2]=VAR(w_1)={\sigma }_{w_1}^2$,同理，$E[w_2^2]={\sigma }_{w_2}^2$，$E[w_1{w}_2]=E[w_2{w}_1]={\sigma }_{w_1}{\sigma }_{w_2}$。
  因此，过程噪声$w$的协方差矩阵Q可以写成：

\qquad \qquad $Q=\left[\begin{array}{cc}E[w_1^2]& E[w_1{w}_2]\\ E[w_2{w}_1]& E[w_2^2]\end{array}\right]=E[w{w}^T]=\left[\begin{array}{cc}{\sigma }_{w_1}^2& {\sigma }_{w_1}{\sigma }_{w_2}\\ {\sigma }_{w_1}{\sigma }_{w_2}& {\sigma }_{w_2}^2\end{array}\right]$。

四、Kalman Filter公式推导

1、离散状态空间方程：

$\{\begin{array}{ll}{x}_k=A{x}_{k-1}+B{u}_{k-1}+w_{k-1}& （4.1）\\ z_k=H{x}_k+v_k& （4.2）\end{array}$

• $A$:状态转移矩阵, $n\times n$维，$B$:输入矩阵, $n\times n$维，$H$:观测矩阵, $m\times n$维, $x_k$：状态变量, $n\times 1$维，$z_k$:测量值, $m\times 1$维，$u_k$：输入信号, $n\times 1$维，$w_{k-1}$：过程噪声, $n\times 1$维，$v_k$:测量噪声, $m\times 1$维。其中，$w_{k-1}、v_k$服从期望为0 ，协方差分别为Q，R的正态分布，即：$w\sim p(0,Q)$，$v\sim p(0,R)$。

2、先验估计、后验估计及其误差

• 先验估计${\hat{x}}_k^{-}$：根据已知的系统模型(没有过程噪声，因为过程噪声是未知的，已知的只有确定的研究对象能够建立的模型)和上一次迭代结果而产生的估计值；

• 后验估计${\hat{x}}_k$：根据当前计算的结果而产生的估计值；

• 定义先验估计误差$e_k^{-}\sim p(0,P_k^{-})：e_k^{-}=x_k-{\hat{x}}_k^{-}$；

• 定义后验估计误差$e_k\sim p(0,P_k)$：$e_k=x_k-{\hat{x}}_k$；

• $x_k$为真实值，带^的为估计值；

• 定义先验估计误差的协方差矩阵$P_k^{-}=E[e_k^{-}{e_k^{-}}^T]$;

• 定义后验估计误差的协方差矩阵$P_k=E[e_k{e_k}^T]$;

• 先验估计和后验估计值计算：
  1、根据${\hat{x}}_{k-1}$计算出$k$时刻的先验估计值${\hat{x}}_k^{-}$: ${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_{k-1}(4.3)$；
  2、测量结果的先验估计值${\hat{x}}_{k_{mea}}=H^{-1}{z}_k$;
  3、根据1)、2)可以计算出$k$时刻的后验估计值${\hat{x}}_k={\hat{x}}_k^{-}+G({\hat{x}}_{k_{mea}}-{\hat{x}}_k^{-})$，令$G=K_{k}H$，则上式变成：
  ${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})(4.4)$；
  4、$K_k\in [0,H^{-}]$: Kalman Gain, $H{\hat{x}}_k^{-}$: 残差。

• 基于以上定义和推导，Kalman Filter的目的即是：在系统结构已知的情况下，给定$k$时刻的状态观测向量$z_k$,求$k$时刻的系统状态向量的最优估计${\hat{x}}_k$,使得后验估计误差协方差矩阵$P$最小(矩阵的迹最小$\to$ 方差最小 $\to$ 后验估计误差最小)。亦即寻找$K_k$,使得估计值${\hat{x}}_k$ 趋近于真实值$x_k$。

3、寻找Kalman Gain$K_k$

\qquad 结合后验估计误差的协方差矩阵公式 $P_k=E[e_k{e}_k^T]$ 和后验估计误差公式 $e_k=x_k-{\hat{x}}_k$可得：
\qquad $P_k=E[(x_k-{\hat{x}}_k)(x_k-{\hat{x}}_k{)}^T](4.5)$

将公式(4.4)代入公式(4.5)中可得：

\qquad $P_k=E[(x_k-{\hat{x}}_k^{-}-K_k{z}_k+K_{k}H{\hat{x}}_k^{-})(x_k-{\hat{x}}_k^{-}-K_k{z}_k+K_{k}H{\hat{x}}_k^{-}{)}^T]$

把$z_k=H{x}_k+v_k$代入上式可得：

\qquad $P_k=E[(x_k-{\hat{x}}_k^{-}-K_k(H{x}_k+v_k))+K_{k}H{\hat{x}}_k^{-})(x_k-{\hat{x}}_k^{-}-K_k(H{x}_k+v_k)+K_{k}H{\hat{x}}_k^{-}{)}^T]$

展开后得：

\qquad $P_k=E[(x_k-{\hat{x}}_k^{-}-K_{k}H{x}_k-K_k{v}_k+K_{k}H{\hat{x}}_k^{-})(x_k-{\hat{x}}_k^{-}-K_{k}H{x}_k-K_k{v}_k+K_{k}H{\hat{x}}_k^{-}{)}^T]$

整理后得：

\qquad $P_k=E[((I-K_{k}H)(x_k-{\hat{x}}_k^{-})-K_k{v}_k)((I-K_{k}H)(x_k-{\hat{x}}_k^{-})-K_k{v}_k{)}^T]$

将先验估计误差$e_k^{-}=x_k-{\hat{x}}_k^{-}$代入上式可得：

\qquad $P_k=E[((I-K_{k}H)e_k^{-}-K_k{v}_k)((I-K_{k}H)e_k^{-}-K_k{v}_k{)}^T]$

• 补充知识：矩阵转置的性质
  1、$(A^T{)}^T=A$；
  2、$(kA{)}^T=k{A}^T$；
  3、$(AB{)}^T=B^T{A}^T$；
  4、$(A\pm B{)}^T=A^T\pm B^T$。

根据矩阵转置性质可得：

\qquad $P_k=E[((I-K_{k}H)e_k^{-}-K_k{v}_k)(e_k^{-T}(I-K_{k}H{)}^T-v_k^T{K}_k^T)]$

$=E[(I-K_{k}H)e_k^{-}{e}_k^{-T}(I-K_{k}H{)}^T-(I-K_{k}H)e_k^{-}{v}_k^T{K}_k^T-K_k{v}_k{e}_k^{-T}(I-K_{k}H{)}^T+K_k{v}_k{v}_k^T{K}_k^T]$

$=E[(I-K_{k}H)e_k^{-}{e}_k^{-T}(I-K_{k}H{)}^T]-E[(I-K_{k}H)e_k^{-}{v}_k^T{K}_k^T]-E[K_k{v}_k{e}_k^{-T}(I-K_{k}H{)}^T]+E[K_k{v}_k{v}_k^T{K}_k^T](4.6)$

由于先验估计误差$e_k^{-}$和测量噪声$v_k$是相互独立且其期望都等于0，根据期望的性质可得：

\qquad $E[(I-K_{k}H)e_k^{-}{v}_k^T{K}_k^T]=E[I-K_{k}H]E[e_k^{-}]E[v_k^T]E[K_k^T]=0$

同理，

\qquad $E[K_k{v}_k{e}_k^T(I-K_{k}H{)}^T]=E[K_k]E[v_k]E[e_k^{-T}]E[(I-K_{k}H{)}^T]=0$

因此，式(4.6)整理后得：

\qquad $P_k=E[I-K_{k}H]E[e_k^{-}{e}_k^{-T}]E[(I-K_{k}H{)}^T]+E[K_k]E[v_k{v}_k^T]E[K_k^T](4.7)$

相对于先验估计误差$e_k^{-}$和噪声$v_k$，$(I-K_{k}H)、K_k$都是常数，此外，$P_k^{-}=E[e_k^{-}{e}_k^{-T}]$为先验估计误差协方差矩阵,且如前所述噪声协方差矩阵$R=E[v_k{v}_k^T]$。所以式(4.7)变为：

\qquad $P_k=(I-K_{k}H)P_k^{-}(I-K_{k}H{)}^T+K_{k}R{K}_k^T$

$=(P_k^{-}-K_{k}H{P}_k^{-})(I-H^T{K}_k^T)+K_{k}R{K}_k^T$

$=P_k^{-}-P_k^{-}{H}^T{K}_k^T-K_{k}H{P}_k^{-}+K_{k}H{P}_k^{-}{H}^T{K}_k^T+K_{k}R{K}_k^T(4.8)$

如前所述，要寻找$K_k$，使得后验估计误差最小，亦即需要求后验估计误差得协方差矩阵$P_k$得迹最小，因此，式(4.8)的迹为：

$tr(P_k)=tr(P_k^{-})-tr(P_k^{-}{H}^T{K}_k^T)-tr(K_{k}H{P}_k^{-})+tr(K_{k}H{P}_k^{-}{H}^T{K}_k^T)+tr(K_{k}R{K}_k^T)(4.9)$

• 补充知识：矩阵迹及其导数性质
  1、$tr(AB)=tr(BA)$;
  2、$tr(ABC)=tr(CAB)=tr(BCA)$;
  3、$tr(A)=tr(A^T)$;
  4、如果$a\in$实数，则有$tr(a)=a$;
  5、$d(tr(XB))=d(tr(BX))=B^{\prime }$，对$X$求导;
  6、$d(tr(X^{\prime }B))=d(tr(B{X}^{\prime }))=B$;
  7、$dtr(X)=I$, $I$为单位矩阵;
  8、$dtr(A^{\prime }X{B}^{\prime })=dtr(B{X}^{\prime }A)=AB$;
  9、$d(tr(AXB{X}^{\prime }))=AXB+A^{\prime }X{B}^{\prime }$;
  10、$dtr(AXBX)=A^{\prime }{X}^{\prime }{B}^{\prime }+B^{\prime }{X}^{\prime }{A}^{\prime }$;

根据矩阵的迹及其导数性质对式(4.9)求导可得：

\qquad $\frac{dtr{P}_k}{d{K}_k}=-P_k^{-}{H}^T-P_k^{-}{H}^T+2K_{k}H{P}_k^{-}{H}^T+2K_{k}R(4.10)$

令式(4.10)等于0可得：

\qquad $K_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}(4.11)$

至此，Kalman Gain $K_k$便已推到完毕。

4、先验估计误差协方差矩阵$P_k^{-}$及后验估计误差协方差矩阵$P_k$推导

• $\begin{array}{r}系统上一次的输出x_{k-1}已知\\ u_{k-1}已知\\ 状态矩阵A、输入矩阵B已知\end{array}\}$ $⟹$先验估计值${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_{k-1}$已知；
  
  
• $\begin{array}{r}先验估计值{\hat{x}}_k^{-}已知\\ 系统输出z_k已知\\ 输出矩阵H已知\end{array}\}$ $⟹$要求后验估计值${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})$，只需求出Kalman Gain$K_k$$\stackrel{式(4.11)}{⟹}$只要求出先验估计误差的协方差矩阵$P_k^{-}$即可。

先验估计误差的协方差矩阵$P_k^{-}$表达式为：

\qquad $P_k^{-}=E[e_k^{-}{e}_k^{-T}]=E[(x_k-{\hat{x}}_k^{-})(x_k-x_k^{-}{)}^T]$

将$x_k=A{x}_{k-1}+B{u}_{k-1}+w_{k-1}$代入上式得：

\qquad $P_k^{-}=E[(A{x}_{k-1}+w_{k-1}-A{\hat{x}}_{k-1})(A{x}_{k-1}+w_{k-1}-A{\hat{x}}_{k-1}{)}^T]$

$=E[(A(x_{k-1}-{\hat{x}}_{k-1})+w_{k-1})(A(x_{k-1}-{\hat{x}}_{k-1})+w_{k-1}{)}^T]$

$=E[(A{e}_{k-1}+w_{k-1})(A{e}_{k-1}+w_{k-1}{)}^T]$

$=E[(A{e}_{k-1}+w_{k-1})(e_{k-1}^T{A}^T+w_{k-1}^T)]$

$=E[A{e}_{k-1}{e}_{k-1}^T{A}^T+A{e}_{k-1}{w}_{k-1}^T+w_{k-1}{e}_{k-1}^T{A}^T+w_{k-1}{w}_{k-1}^T]$

$=E[A{e}_{k-1}{e}_{k-1}^T{A}^T]+E[A{e}_{k-1}{w}_{k-1}^T]+E[w_{k-1}{e}_{k-1}^T{A}^T]+E[w_{k-1}{w}_{k-1}^T](4.12)$

由于系统上一次得后验估计误差$e_{k-1}$与系统本次的过程噪声$w_{k-1}$是相互独立且期望都为0，则$E[A{e}_{k-1}{w}_{k-1}^T]=E[w_{k-1}{e}_{k-1}^T{A}^T]=0$,因此式(4.12)可变为：

$P_k^{-}=E[A]E[e_{k-1}{e}_{k-1}^T]E[A^T]+E[w_{k-1}{w}_{k-1}^T]$

$=A{P}_{k-1}{A}^T+Q(4.13)$

把式(4.11)代入式(4.8)中可得：

\qquad $P_k=P_k^{-}-P_k^{-}{H}^T{K}_k^T-K_{k}H{P}_k^{-}+K_k(H{P}_k^{-}{H}^T+R)K_k^T$

$=P_k^{-}-P_k^{-}{H}^T{K}_k^T-K_{k}H{P}_k^{-}+\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}(H{P}_k^{-}{H}^T+R)K_k^T$

$=P_k^{-}-P_k^{-}{H}^T{K}_k^T-K_{k}H{P}_k^{-}+P_k^{-}{H}^T{K}_k^T$

$=P_k^{-}-K_{k}H{P}_k^{-}$

$=(I-K_{k}H)P_k^{-}4.(14)$

五、Kalman Filter计算步骤

Kalman Filter 实施分成两大部分：

1、预测

1、计算先验估计值：${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_{k-1}(4.3)$

2、计算先验误差协方差矩阵$P_k^{-}=A{P}_{k-1}{A}^T+Q(4.13)$

2、校正

3、计算Kalman Gain $K_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}(4.11)$

4、计算后验估计值 ${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})(4.4)$

5、更新后验估计误差协方差矩阵 $P_k=(I-K_{k}H)P_k^{-}(4.14)$

六、Kalman Filter实例之PMSM速度观测器设计

• PMSM:永磁同步电机

• 补充知识：控制系统常用的离散化方法
  1、 前向差分法：
  \qquad 一阶前向差分：$\frac{du}{dt}\approx \frac{u(k+1)-u(k)}{T}$
  2、 后向差分法
  \qquad 一阶后向差分：$\frac{du}{dt}\approx \frac{u(k)-u(k-1)}{T}$
  3、 双线性变换法
  4、 脉冲响应不变法
  5、 阶跃响应不变法
  6、 零极点匹配法

1、PMSM离散化空间状态方程

• PMSM电磁转矩方程

\qquad $T_e=\frac{3}{2}p({\psi }_d{i}_q-{\psi }_q{i}_d)(6.1)$

其中$p$为电机极对数，${\psi }_d、{\psi }_q$为$d、q$轴磁链，$i_d、i_q$为$d、q$轴电流。把${\psi }_d={\psi }_f+L_d{i}_d,{\psi }_q=L_q{i}_q$代入式(6.1)中可得:

$T_e=\frac{3}{2}p((({\psi }_f+L_d{i}_d)i_q)-L_q{i}_q{i}_d)$

$=\frac{3}{2}p({\psi }_f{i}_q+(L_d-L_q)i_d{i}_q)(6.2)$

其中$L_d、L_q$分别为$d、q$轴电感，${\psi }_f$为永磁体磁链。根据PMSM矢量控制算法，通常都令$i_d=0$,则式(6.2)变为：

$T_e=\frac{3}{2}p{\psi }_f{i}_q(6.3)$

• PMSM机械运动方程

$T_e-T_L-B{\omega }_m=J\frac{d{\omega }_m}{dt}(6.4)$

其中$T_L$为负载转矩，$B$为摩擦系数。如果忽略摩擦因素，则式(6.4)变为：

$T_e-T_L=J\frac{d{\omega }_m}{dt}(6.5)$

此外，假设在$dt$时间内负载转矩$T_L$不变，即：

$\frac{d{T}_L}{dt}=0(6.6)$

联合式(6.5)、(6.6)可得PMSM转速与负载转矩的状态空间方程：

• $\{\begin{array}{l}\left[\begin{array}{c}\frac{d{\omega }_m}{dt}\\ \\ \frac{d{T}_L}{dt}\end{array}\right]=\left[\begin{array}{cc}0& -\frac{1}{J}\\ & \\ 0& 0\end{array}\right]\left[\begin{array}{c}{\omega }_m\\ \\ T_L\end{array}\right]+\left[\begin{array}{c}\frac{1.5p{\psi }_f}{J}\\ \\ 0\end{array}\right]i_q(6.7)\\ \\ {\omega }_m=\left[\begin{array}{ccc}1& & 0\end{array}\right]\left[\begin{array}{c}{\omega }_m\\ \\ T_L\end{array}\right](6.8)\end{array}$

2、状态空间方程离散化

将式(6.7)进行一阶后向差分离散化得：

\qquad $\left[\begin{array}{c}\frac{{\omega }_m(k)-{\omega }_m(k-1)}{T_s}\\ \\ \frac{T_L(k)-T_L(k-1)}{T_s}\end{array}\right]=\left[\begin{array}{cc}0& -\frac{1}{J}\\ & \\ 0& 0\end{array}\right]\left[\begin{array}{c}{\omega }_m(k-1)\\ \\ T_L(k-1)\end{array}\right]+\left[\begin{array}{c}\frac{1.5p{\psi }_f}{J}\\ \\ 0\end{array}\right]i_q(k-1)$

整理后得：

\qquad $\begin{array}{c}\underbrace{\left[\begin{array}{c}{\omega }_m(k)\\ \\ T_L(k)\end{array}\right]}\\ x_k\end{array}=\begin{array}{c}\underbrace{\left[\begin{array}{cc}1& -\frac{T_s}{J}\\ & \\ 0& 1\end{array}\right]}\\ A\end{array}\begin{array}{c}\underbrace{\left[\begin{array}{c}{\omega }_m(k-1)\\ \\ T_L(k-1)\end{array}\right]}\\ x_{k-1}\end{array}+\begin{array}{c}\underbrace{\left[\begin{array}{c}\frac{1.5p{\psi }_f{T}_s}{J}\\ \\ 0\end{array}\right]}\\ B\end{array}\begin{array}{c}\underbrace{i_q(k-1)}\\ u_{k-1}\end{array}(6.9)$

将式(6.8)进行一阶后向离散化得：

\qquad $\begin{array}{c}\underbrace{{\omega }_m(k)}\\ z_k\end{array}=\begin{array}{c}\underbrace{\left[\begin{array}{cc}1& 0\end{array}\right]}\\ H\end{array}\begin{array}{c}\underbrace{\left[\begin{array}{c}{\omega }_m(k)\\ \\ T_L(k)\end{array}\right]}\\ x_k\end{array}(6.10)$

3、Kalman Filter 观测器设计

\quad 假设PMSM在运行过程中引入了过程噪声$w$和编码器测量时候的测量噪声为$v$,且$w\sim p(0,Q)、v\sim p(0,R)$，则式(6.9)、(6.10)可以改写成：

\qquad $\{\begin{array}{l}{x}_k=A{x}_{k-1}+B{u}_{k-1}+w_{k-1}\\ z_k=H{x}_k+v_k\end{array}$

假设某一PMSM具有如下特性：极对数$p=2$，$J=2.7\times 10^{-5}kg\cdot m^2$，采样周期$T_s=2ms$，永磁体磁链${\psi }_f=0.162Wb$。则矩阵$A、B$分别等于：

\qquad $A=\left[\begin{array}{cc}1& -74.07\\ 0& 1\end{array}\right]，B=\left[\begin{array}{c}36\\ 0\end{array}\right]$

初始化 ${\omega }_m(0)=0，i_q(0)=0，T_L(0)=0，P_0=0$。
\qquad 当$k=1$时，进行第一次迭代，根据第六节的计算步骤:
1、计算先验估计值 ${\hat{x}}_1^{-}=0$；
2、计算先验误差协方差矩阵 $P_1^{-}=Q$;
3、计算Kalman Gain $K_1=\frac{P_1^{-}H}{H{P}_1^{-}{H}^T+R}=\frac{QH}{HQ{H}^T+R}$