无刷电机FOC扩展卡尔曼滤波位置估计matlab仿真，已经放到微信公众号：BOBO的实验小课堂
引入
卡尔曼滤波是通过对下一时刻系统的状态估计以及测量得出的反馈相结合，最终得到该时刻较为准确的的状态估计（预测+测量反馈），我们一般所说的卡尔曼滤波KF是针对线性系统的，其思想对于非线性系统也是适应的，由此延伸出来的有EKF，UKF等。
我们先从简单的问题入手，来了解卡尔曼滤波的思想
假如我们想知道一枚硬币的厚度，并且有k个测量值 z 1 、 z 2 … z k z_1、z_2…z_k z1、z2…zk，为了估计真实值，我们一般对其取均值，于是估计量
x ^ k = 1 k ( z 1 + z 2 … + z k ) = 1 k k − 1 k − 1 ( z 1 + z 2 … + z k − 1 ) + 1 k z k = k − 1 k x ^ k − 1 + 1 k z k x ^ k = x ^ k − 1 + 1 k ( z k − x ^ k − 1 ) = x ^ k − 1 + K k ( z k − x ^ k − 1 ) \hat{x}_k = \frac{1}{k}(z_1+z_2…+z_k) = \frac{1}{k}\frac{k-1}{k-1}(z_1+z_2…+z_{k-1}) +\frac{1}{k}z_k = \frac{k-1}{k}\hat{x}_{k-1}+\frac{1}{k}z_k\\ \hat{x}_k = \hat{x}_{k-1}+\frac{1}{k}(z_k - \hat{x}_{k-1}) = \hat{x}_{k-1}+K_k(z_k - \hat{x}_{k-1}) x^k=k1(z1+z2…+zk)=k1k−1k−1(z1+z2…+zk−1)+k1zk=kk−1x^k−1+k1zkx^k=x^k−1+k1(zk−x^k−1)=x^k−1+Kk(zk−x^k−1) ①
由①式知，当我们只有一个测量值时，即k = 1，我们只能选择相信测量值，而当 k — > ∞ k —>\infty k—>∞时，我们可以相信上次的估计值。且本次的估计值只跟本次的测量值和上次的估计值有关，不用再追溯到前 k − 1 k-1 k−1个测量值
其中 K k K_k Kk为卡尔曼增益，为使估计误差最小，只要使方差 σ 2 x ^ k \sigma^2\hat{x}_k σ2x^k最小，令 ∂ σ 2 x k ^ ∂ K k = 0 \frac{\partial{\sigma^2\hat{x_k}}}{\partial{K_k}} = 0 ∂Kk∂σ2xk^=0得 K k = σ 2 x ^ k − 1 σ 2 x ^ k − 1 + σ 2 z ^ k K_k = \frac{\sigma^2\hat{x}_{k-1}}{\sigma^2\hat{x}_{k-1}+\sigma^2\hat{z}_{k}} Kk=σ2x^k−1+σ2z^kσ2x^k−1，并做下式变换得卡尔曼增益的一般形式，此 K k K_k Kk值具有数学上的最优性
K k = e E S T [ k − 1 ] e E S T [ k − 1 ] + e M E A [ k ] K_k = \frac{e_{EST}[k-1]}{e_{EST}[k-1]+e_{MEA}[k]} Kk=eEST[k−1]+eMEA[k]eEST[k−1] ②
e
E
S
T
e_{EST}
eEST为估计误差，
e
M
E
A
e_{MEA}
eMEA为测量误差。观察②式知，当
e
E
S
T
≫
e
M
E
A
e_{EST}≫e_{MEA}
eEST≫eMEA时，
K
k
=
1
K_k = 1
Kk=1，将带入①式
x
^
k
=
z
k
\hat{x}_k =z_k
x^k=zk，即当估计误差比较大时我们选择相信测量值。相反，当
e
E
S
T
≪
e
M
E
A
e_{EST}≪e_{MEA}
eEST≪eMEA时，
K
k
=
0
K_k = 0
Kk=0，
x
^
k
=
x
^
k
−
1
\hat{x}_k =\hat{x}_{k-1}
x^k=x^k−1，此时我们选择相信估计值
该情形下卡尔曼滤波的一般步骤：
| 步骤 | 公式 |
|---|---|
| step 1 | K k = e E S T [ k − 1 ] e E S T [ k − 1 ] + e M E A [ k ] K_k = \frac{e_{EST}[k-1]}{e_{EST}[k-1]+e_{MEA}[k]} Kk=eEST[k−1]+eMEA[k]eEST[k−1] |
| step 2 | x ^ k = x ^ k − 1 + K k ( z k − x ^ k − 1 ) \hat{x}_k = \hat{x}_{k-1}+K_k(z_k - \hat{x}_{k-1}) x^k=x^k−1+Kk(zk−x^k−1) |
| step 3 | e E S T [ k ] = ( 1 − K k ) e E S T [ k − 1 ] e_{EST}[k] = (1-K_k)e_{EST}[k-1] eEST[k]=(1−Kk)eEST[k−1] |
卡尔曼滤波器
考虑系统
{ x ⃗ [ k ] = A x ⃗ [ k − 1 ] + B u ⃗ [ k − 1 ] + w [ k − 1 ] z ⃗ = H x ⃗ [ k ] + v [ k ] \left\{ \begin{array}{ll} \vec{x}[k] = A\vec{x}[k-1] + B \vec{u}[k-1] +w[k-1] \\ \vec{z} = H\vec{x}[k]+v[k] \end{array} \right. {x[k]=Ax[k−1]+Bu[k−1]+w[k−1]z=Hx[k]+v[k] ③
其中ω为过程噪声，服从N(0,Q)分布。υ为测量噪声，服从N(0,R)分布
1、协方差矩阵
将方差和协方差通过一个矩阵进行表示
方差： σ x 2 = 1 n − 1 ∑ i = 1 n ( x i − x ˉ ) 2 σ^2_x = \frac{1}{n-1}\sum_{i=1}^n(x_i - \bar{x})^2 σx2=n−11∑i=1n(xi−xˉ)2
协方差： σ x , y = 1 n − 1 ∑ i = 1 n ( x i − x ˉ ) ( y i − y ˉ ) σ_{x,y} = \frac{1}{n-1}\sum_{i=1}^n(x_i - \bar{x})(y_i - \bar{y}) σx,y=n−11∑i=1n(xi−xˉ)(yi−yˉ)
考虑以下三阶的协方差矩阵
P = [ σ 2 x σ x σ y σ x σ z σ y σ x σ 2 y σ y σ z σ z σ x σ z σ y σ 2 y ] P = \begin{bmatrix} σ^2x&σxσy&σxσz \\ σyσx&σ^2y&σyσz \\ σzσx&σzσy&σ^2y \end{bmatrix} P= σ2xσyσxσzσxσxσyσ2yσzσyσxσzσyσzσ2y
构造过度矩阵T求此协方差矩阵， 1 3 o n e s ( 3 , 3 ) [ x , y , z ] \frac{1}{3} ones(3,3)[x,y,z] 31ones(3,3)[x,y,z]构成了 x , y , z x,y,z x,y,z的平均值矩阵
T = [ x 1 y 1 z 1 x 2 y 2 z 2 x 3 y 3 z 3 ] − 1 3 [ 1 1 1 1 1 1 1 1 1 ] [ x 1 y 1 z 1 x 2 y 2 z 2 x 3 y 3 z 3 ] T = \begin{bmatrix}x_1&y_1&z_1 \\x_2&y_2&z_2 \\x_3&y_3&z_3\end{bmatrix} - \frac{1}{3} \begin{bmatrix}1&1&1 \\1&1&1 \\1&1&1\end{bmatrix} \begin{bmatrix}x_1&y_1&z_1 \\x_2&y_2&z_2 \\x_3&y_3&z_3\end{bmatrix} T= x1x2x3y1y2y3z1z2z3 −31 111111111 x1x2x3y1y2y3z1z2z3
P = 1 3 T T T P = \frac{1}{3}T^TT P=31TTT
对于③式表示的系统
由于 D ( X ) = E ( X 2 ) − E 2 ( X ) D(X) = E(X^2) -E^2(X) D(X)=E(X2)−E2(X)，而 E ( ω i ) = 0 E(ω_i) = 0 E(ωi)=0，于是协方差矩阵可由下式表示：
Q = E ( ω ω T ) = E ( [ w 1 ⋮ w n ] [ w 1 … w n ] ) = [ E ( w 1 2 ) E ( w 1 w 2 ) … E ( w 1 w n ) E ( w 2 w 1 ) E ( w 2 2 ) … E ( w 2 w n ) ⋮ ⋮ ⋮ E ( w n w 1 ) E ( w n w 2 ) … E ( w n 2 ) ] ) Q = E(ωω^T) = E(\begin{bmatrix}w_1 \\ \vdots \\w_n \end{bmatrix} \begin{bmatrix} w_1 &…& w_n \end{bmatrix}) = \begin{bmatrix} E(w_1^2) & E(w_1w_2)&…& E(w_1w_n) \\ E(w_2w_1) & E(w_2^2)&…& E(w_2w_n) \\ \vdots & \vdots& & \vdots \\ E(w_nw_1) & E(w_nw_2)&…& E(w_n^2) \\ \end{bmatrix}) Q=E(ωωT)=E( w1⋮wn [w1…wn])= E(w12)E(w2w1)⋮E(wnw1)E(w1w2)E(w22)⋮E(wnw2)………E(w1wn)E(w2wn)⋮E(wn2) )
同时，ω1、ω2、… 、ωn相互独立
Q = [ σ w 1 2 σ w 1 , w 2 … σ w 1 , w n σ w 2 , w 1 σ w 2 2 … σ w 2 , w n ⋮ ⋮ ⋮ σ w n , w 1 σ w n , w 2 … σ w n 2 ] = [ σ w 1 2 0 … 0 0 σ w 2 2 … 0 ⋮ ⋮ ⋮ 0 0 … σ w n 2 ] Q = \begin{bmatrix} σ_{w_1}^2 & σ_{w_1,w_2}&…& σ_{w_1,w_n} \\σ_{w_2,w_1} & σ_{w_2}^2&…& σ_{w_2,w_n} \\\vdots & \vdots& & \vdots \\σ_{w_n,w_1} & σ_{w_n,w_2}&…& σ_{w_n}^2 \\ \end{bmatrix} =\begin{bmatrix} σ_{w_1}^2 &0&…& 0 \\ 0 & σ_{w_2}^2&…& 0 \\ \vdots & \vdots& & \vdots \\ 0 & 0&…& σ_{w_n}^2 \\ \end{bmatrix} Q= σw12σw2,w1⋮σwn,w1σw1,w2σw22⋮σwn,w2………σw1,wnσw2,wn⋮σwn2 = σw120⋮00σw22⋮0………00⋮σwn2
同理 R = E ( υ υ T ) R = E(υυ^T) R=E(υυT)
2、先验估计
没有考虑过程误差和测量误差，由状态空间方程计算所得
x ⃗ ^ − [ k ] = A x ⃗ [ k − 1 ] + B u ⃗ [ k − 1 ] \hat{\vec{x}}^-[k] = A\vec{x}[k-1 ]+B\vec{u}[k-1] x^−[k]=Ax[k−1]+Bu[k−1]
但是在电机模型中 ω 和 θ ω和θ ω和θ无法获得，我们可以将 x ⃗ [ k − 1 ] \vec{x}[k-1 ] x[k−1]替换为 x ⃗ ^ [ k − 1 ] \hat{\vec{x}}[k-1 ] x^[k−1]
同时由测量 z ⃗ = H x ⃗ [ k ] \vec{z} = H\vec{x}[k] z=Hx[k]得，
x ⃗ ^ [ k − m e a ] = H − 1 z ⃗ [ k ] \hat{\vec{x}}[k-mea] = H^{-1}\vec{z}[k] x^[k−mea]=H−1z[k]
3、后验估计
估计+测量反馈
x ⃗ ^ [ k ] = x ⃗ ^ − [ k ] + G ( H − 1 z ⃗ [ k ] − x ⃗ ^ − [ k ] ) \hat{\vec{x}}[k] = \hat{\vec{x}}^-[k] + G(H^{-1}\vec{z}[k] - \hat{\vec{x}}^-[k]) x^[k]=x^−[k]+G(H−1z[k]−x^−[k])
令 G = K k H G = K_kH G=KkH，则
x ⃗ ^ [ k ] = x ⃗ ^ − [ k ] + K k ( z ⃗ [ k ] − H x ⃗ ^ − [ k ] ) \hat{\vec{x}}[k] = \hat{\vec{x}}^-[k] + K_k(\vec{z}[k] - H\hat{\vec{x}}^-[k]) x^[k]=x^−[k]+Kk(z[k]−Hx^−[k])
4、卡尔曼增益
卡尔曼增益 K k K_k Kk是凹函数的极点，因此卡尔曼滤波器既是一个观测器，又是最优性估计
令 e ⃗ k = x ⃗ k − x ⃗ ^ k \vec{e}_k = \vec{x}_k-\hat{\vec{x}}_k ek=xk−x^k，服从N(0,P)分布
P = E ( e k e k T ) P = E(e_ke_k^T) P=E(ekekT)
为了使 t r ( P ) tr(P) tr(P)最小，即方差最小，令 ∂ t r ( P ) ∂ K k = 0 \frac{\partial{tr(P)}}{\partial{K_k}} = 0 ∂Kk∂tr(P)=0得
K k = P − [ k ] H T H P − [ k ] H T + R K_k= \frac{P^-[k]H^T}{HP^-[k]H^T+R} Kk=HP−[k]HT+RP−[k]HT
具体推导过程可参考B站UP主DR_CAN，视频链接放在文末
其中 P − P^- P−为先验误差
P − = E ( e k − e k − T ) = A P k − 1 A T + Q P^- = E(e_k^-e_k^{-T}) = AP_{k-1}A^T+Q P−=E(ek−ek−T)=APk−1AT+Q， e k ⃗ − = x ⃗ k − x ⃗ ^ k − \vec{e_k}^- = \vec{x}_k-\hat{\vec{x}}^-_k ek−=xk−x^k−
卡尔曼滤波的一般步骤
| 预测 | 矫正 | ||
|---|---|---|---|
| ①先验估计 | x ⃗ ^ − [ k ] = A x ⃗ [ k − 1 ] + B u ⃗ [ k − 1 ] \hat{\vec{x}}^-[k] = A\vec{x}[k-1] + B \vec{u}[k-1] x^−[k]=Ax[k−1]+Bu[k−1] | ③计算卡尔曼增益 | K k = P − [ k ] H T H P − [ k ] H T + R K_k= \frac{P^-[k]H^T}{HP^-[k]H^T+R} Kk=HP−[k]HT+RP−[k]HT |
| ②先验误差协方差矩阵 | P k − = E ( e k − e k − T ) = A P k − 1 A T + Q P_k^- = E(e_k^-e_k^{-T}) = AP_{k-1}A^T+Q Pk−=E(ek−ek−T)=APk−1AT+Q | ④后验估计 | x ⃗ ^ [ k ] = x ⃗ ^ − [ k ] + K k ( z ⃗ [ k ] − H x ⃗ ^ − [ k ] ) \hat{\vec{x}}[k] = \hat{\vec{x}}^-[k] + K_k(\vec{z}[k] - H\hat{\vec{x}}^-[k]) x^[k]=x^−[k]+Kk(z[k]−Hx^−[k]) |
| ⑤更新误差协方差矩阵 | P k = ( I − K k H ) P k − P_k = (I - K_kH)P_k^- Pk=(I−KkH)Pk− |
电机状态方程
1、状态向量
上文龙伯格观测器中我们选取了状态向量 x ⃗ = [ i α i β u α u β ] T \vec{x} = \begin{bmatrix}i_α&i_β&u_α&u_β\end{bmatrix}^T x=[iαiβuαuβ]T，但是为了利用卡尔曼滤波器对ω和θ进行预测我们选取状态向量 x ⃗ = [ i α i β w θ ] T \vec{x} = \begin{bmatrix}i_α&i_β&w&θ\end{bmatrix}^T x=[iαiβwθ]T，测量向量 z ⃗ = [ i α i β ] T \vec{z} = \begin{bmatrix}i_α&i_β\end{bmatrix}^T z=[iαiβ]T，并将反电动势形式做如下修改（电流滞后电压90°，因此α轴的反电动势由β轴电压产生，并且反向）
{ e α = − k e w e s i n ( w e t ) e β = k e w e c o s ( w e t ) \left\{ \begin{array}{ll} e_α = -k_e w_e sin(w_et) \\ e_β = k_e w_e cos(w_et) \end{array} \right. {eα=−kewesin(wet)eβ=kewecos(wet) ④
{ d i α d t = − r L S i α + 1 L S k e w e s i n ( θ ) + 1 L S u α d i β d t = − r L S i β − 1 L S k e w e c o s ( θ ) + 1 L S u β d w e d t = 0 d θ d t = w e \left\{ \begin{array}{ll} \frac{di_α}{dt} = -\frac{r}{L_S}i_α +\frac{1}{L_S}k_e w_e sin(θ) + \frac{1}{L_S}u_α \\ \frac{di_β}{dt} = -\frac{r}{L_S}i_β -\frac{1}{L_S}k_e w_e cos(θ) + \frac{1}{L_S}u_β \\ \frac{dw_e}{dt} = 0 \\ \frac{dθ}{dt} = w_e \end{array} \right. ⎩ ⎨ ⎧dtdiα=−LSriα+LS1kewesin(θ)+LS1uαdtdiβ=−LSriβ−LS1kewecos(θ)+LS1uβdtdwe=0dtdθ=we ⑤
2、线性化
此时该系统是一个非线性系统，而经非线性系统的高斯噪声不再服从正态分布，为了利用卡尔曼滤波器我们要先对其进行线性化。用EKF对系统状态进行估计，令
G ( x ⃗ ) = [ − r L S i α + 1 L S k e w e s i n ( θ ) − r L S i β − 1 L S k e w e c o s ( θ ) 0 w e ] G(\vec{x}) = \begin{bmatrix} -\frac{r}{L_S}i_α +\frac{1}{L_S}k_e w_e sin(θ) \\ -\frac{r}{L_S}i_β -\frac{1}{L_S}k_e w_e cos(θ) \\ 0 \\ w_e \end{bmatrix} G(x)= −LSriα+LS1kewesin(θ)−LSriβ−LS1kewecos(θ)0we
在计算先验估计时用的是原系统的非线性函数
对上面卡尔曼滤波器计算的一般步骤①先验估计做如下更正
x ⃗ ^ − [ k ] = x ⃗ ^ [ k − 1 ] + T G ( x ⃗ ) + B u ⃗ [ k − 1 ] \hat{\vec{x}}^-[k] = \hat{\vec{x}}[k-1] + T G(\vec{x}) + B\vec{u}[k-1] x^−[k]=x^[k−1]+TG(x)+Bu[k−1]
雅可比矩阵
J = ▽ x f = d f ( x ) d x = [ ∂ f ( x ) ∂ x 1 … ∂ f ( x ) ∂ x n ] J = \bigtriangledown_x f = \frac{df(x)}{dx} = \begin{bmatrix} \frac{\partial f(x)}{\partial x_1}& … &\frac{\partial f(x)}{\partial x_n} \end{bmatrix} J=▽xf=dxdf(x)=[∂x1∂f(x)…∂xn∂f(x)]
= [ ∂ f 1 ( x ) ∂ x 1 … ∂ f 1 ( x ) ∂ x n ⋮ ⋮ ∂ f n ( x ) ∂ x 1 … ∂ f n ( x ) ∂ x n ] = \begin{bmatrix} \frac{\partial f_1(x)}{\partial x_1}& … &\frac{\partial f_1(x)}{\partial x_n} \\ \vdots&&\vdots \\ \frac{\partial f_n(x)}{\partial x_1}& … &\frac{\partial f_n(x)}{\partial x_n} \\ \end{bmatrix} = ∂x1∂f1(x)⋮∂x1∂fn(x)……∂xn∂f1(x)⋮∂xn∂fn(x)
于是
∂ G ( x ⃗ ) x ⃗ = [ − r L S 0 k e s i n ( θ ) L S k e w e c o s ( θ ) L S 0 − r L S − k e c o s ( θ ) L S + k e w e s i n ( θ ) L S 0 0 0 0 0 0 1 0 ] \frac{\partial G(\vec{x})} {\vec{x}} = \begin{bmatrix} -\frac{r}{L_S}&0&\frac{k_esin(θ)}{L_S}&\frac{k_ew_ecos(θ)}{L_S} \\ 0&-\frac{r}{L_S}&-\frac{k_ecos(θ)}{L_S}&+\frac{k_ew_esin(θ)}{L_S} \\ 0&0&0&0 \\ 0&0&1&0\\ \end{bmatrix} x∂G(x)= −LSr0000−LSr00LSkesin(θ)−LSkecos(θ)01LSkewecos(θ)+LSkewesin(θ)00
3、离散化
{ i α [ k ] = ( 1 − r T L S ) i α [ k − 1 ] − T k e c o s ( θ k − 1 ) L S w e [ k − 1 ] + T k e w e [ k − 1 ] s i n ( θ k − 1 ) L S θ [ k − 1 ] + T L S u α [ k − 1 ] i β [ k ] = ( 1 − r T L S ) i β [ k − 1 ] − T k e s i n ( θ k − 1 ) L S w e [ k − 1 ] − T k e w e [ k − 1 ] c o s ( θ k − 1 ) L S θ [ k − 1 ] + T L S u β [ k − 1 ] w e [ k ] = w e [ k − 1 ] θ [ k ] = θ [ k − 1 ] + T w e [ k − 1 ] \left\{ \begin{array}{ll} i_α[k] = (1-\frac{rT}{L_S})i_α[k-1] -\frac{Tk_e cos(θ_{k-1})}{L_S}w_e[k-1] +\frac{Tk_e w_e[k-1] sin(θ_{k-1})}{L_S}θ[k-1]+ \frac{T}{L_S}u_α[k-1] \\ \\ i_β[k] = (1-\frac{rT}{L_S})i_β[k-1] -\frac{Tk_e sin(θ_{k-1})}{L_S}w_e[k-1] - \frac{Tk_e w_e[k-1] cos(θ_{k-1})}{L_S}θ[k-1]+ \frac{T}{L_S}u_β[k-1] \\ \\ w_e[k] = w_e[k - 1] \\\\ θ[k] = θ[k - 1] + Tw_e[k-1] \end{array} \right. ⎩ ⎨ ⎧iα[k]=(1−LSrT)iα[k−1]−LSTkecos(θk−1)we[k−1]+LSTkewe[k−1]sin(θk−1)θ[k−1]+LSTuα[k−1]iβ[k]=(1−LSrT)iβ[k−1]−LSTkesin(θk−1)we[k−1]−LSTkewe[k−1]cos(θk−1)θ[k−1]+LSTuβ[k−1]we[k]=we[k−1]θ[k]=θ[k−1]+Twe[k−1]
于是线性化后的离散系统矩阵
A = [ 1 − r T L S 0 T k e s i n ( θ k − 1 ) L S T k e w e [ k − 1 ] c o s ( θ k − 1 ) L S 0 1 − r T L S − T k e c o s ( θ k − 1 ) L S T k e w e [ k − 1 ] s i n ( θ k − 1 ) L S 0 0 1 0 0 0 T 1 ] A = \begin{bmatrix} 1-\frac{rT}{L_S}&0&\frac{Tk_e sin(θ_{k-1})}{L_S}&\frac{Tk_e w_e[k-1] cos(θ_{k-1})}{L_S} \\ 0 & 1-\frac{rT}{L_S}&-\frac{Tk_e cos(θ_{k-1})}{L_S}&\frac{Tk_e w_e[k-1] sin(θ_{k-1})}{L_S} \\ 0&0&1&0 \\ 0&0&T&1\\ \end{bmatrix} A= 1−LSrT00001−LSrT00LSTkesin(θk−1)−LSTkecos(θk−1)1TLSTkewe[k−1]cos(θk−1)LSTkewe[k−1]sin(θk−1)01
B = [ T L S 0 0 T L S 0 0 0 0 ] B = \begin{bmatrix} \frac{T}{L_S}&0 \\ 0&\frac{T}{L_S} \\ 0&0 \\ 0&0\\ \end{bmatrix} B= LST0000LST00
H = [ 1 0 0 0 0 1 0 0 ] H = \begin{bmatrix} 1&0&0&0 \\ 0&1&0&0 \end{bmatrix} H=[10010000]
MATLAB仿真
仿真模型开源自取（微信公众号：BOBO的实验小课堂）
可以看到卡尔曼滤波器的θ估计输出几乎与实际位置重合，但速度估计ω受磁链常数影响，磁链给的偏大会导致ω估计偏小，相反磁链给的偏小会导致ω估计偏大，对速度的估计并不准确，推荐对θ微分作为ω的估计输出
单片机实际运行效果
如图，在10KHz采样频率下，电流和位置都得到了很好的跟踪，其中xd0和xd3分别为ialpha_hat与theta_hat，foc.ialpha和foc.theta为实际值，观测波形采用的是CubeMonitor，其最大采样率只有1KHz，故曲线并不是很顺滑
Reference
[1] PMSM之FOC控制13-搭建EKF观测器
[2] 扩展卡尔曼滤波（EKF）理论讲解与实例(matlab、python和C++代码)
[3] B站. DR_CAN. 3_卡尔曼增益超详细数学推导
[4] 卡尔曼滤波（KF）和扩展卡尔曼滤波（EKF）相应推导
[5] 电机FOC无感位置估计（一）：龙伯格观测器
扫一扫
1699
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
