控制系统 状态观测问题 Kalman卡尔曼(附MATLAB实现)
最近有同学问我卡尔曼滤波的问题，发现有些细节部分已经忘了，整理一下之前的笔记并结合现代控制工程的内容写成blog
对控制工程的理解
控制工程分为两部分：控制器和被控系统。
控制器
控制器接受期望输出，计算出期望输入
被控系统接受期望输入，得到实际输出
如果控制器计算期望输入的过程不参考上一时刻的实际输出，那就是开环控制，反之则是闭环控制
PID控制器是将整个被控系统作为一个黑盒子，只参考期望输出和实际输出的误差来计算下一时刻的期望输入
状态反馈控制器则是采集参考被控系统中的多个数据来计算下一时刻的输入
响应速度分析，稳定性分析之类的，都是为了调整被控系统，使得被控系统相应更快。
以上说的都是线性系统，对于非线性系统，需要进行线性化才能使用这些方法
被控系统建模
被控系统建模方法很多，同一系统在时域、频域、和复数域中有不同表达方式
对于一个后面挂着弹簧和阻尼器的小车线性系统 F − k x − b v = m a F - kx - bv = ma F−kx−bv=ma建模。
微分方程建模：
m x ¨ + k x + b x ˙ = F ( t ) m\ddot{x}+ kx + b\dot{x}= F(t) mx¨+kx+bx˙=F(t)
传递函数建模(将微分方程通过拉普拉斯变换转换到复数域)：
G ( s ) = Y ( s ) X ( s ) = 1 m s 2 + b s + k G(s) = \frac{Y(s)}{X(s)} = \frac{1}{ms^2+bs+k} G(s)=X(s)Y(s)=ms2+bs+k1
状态空间建模
X ˙ = [ 0 1 − k m − b m ] X + [ 0 1 m ] U X = [ x v ] , U = [ F ( t ) ] \begin{align}\dot{X} & = \begin{bmatrix}0 & 1\\-\frac{k}{m} & -\frac{b}{m} \end{bmatrix}X + \begin{bmatrix}0 \\\frac{1}{m} \end{bmatrix}U \\X & = \begin{bmatrix}x \\v\end{bmatrix},U=\left [ F(t) \right ] \end{align} X˙X=[0−mk1−mb]X+[0m1]U=[xv],U=[F(t)]
如果不能更改被控系统，那就没有后续那些响应速度分析，稳定性分析的事情了，他们都是为了设计一个更好的控制系统而服务的，就比如上面的小车，添加上PID控制器后，可以写出一个新的传递函数，通过控制PID的参数来提高新的传递函数的响应速度和稳定性，这就需要伯德图，奈奎斯特图，极点配置等等了。
这个时候的系统，输入是期望输出，输出是实际输出，输出跟随输入。
也可以把配置后的系统看作一个被控系统，为这个系统再加一个控制器，这就是串级控制了，例如伺服系统中的三环控制
状态观测问题引入
只要是闭环控制器，就需要参考被控系统的中间状态变量或者输出，在PID中就是输出，在状态空间建模中就是状态变量。
因为成本问题或者技术问题，有时候难以直接测量，这就用到观测器了，观测器就是观测系统状态的。
观测器有很多种，如状态观测器、干扰观测器、滑模观测器、扩张状态观测器等
得到第一手数据的方法有两种，
- 建立理论模型推导结果，例如 x = v t + 1 2 a t 2 x = vt + \frac{1}{2}at^2 x=vt+21at2，但是理论模型跟实际值存在误差
这个误差不仅仅是考虑因素的误差，也存在自然界中无法建模的误差，例如考虑到了摩擦，但是在自然界一段路面的摩擦因数是不可能完全一样的，这种就符合高斯误差，所以这也是为什么卡尔曼中的误差假定为高斯误差。
- 安装传感器检测，肯定也是有误差的，而且有的数据也没法用传感器检测
龙伯格观测器
龙伯格就属于建立模型推导结果的方法。
对于状态空间模型，观测器分为全维观测器和降维观测器，龙伯格是全维观测器。
对于一个状态空间方程描述的系统：
{ x ˙ = A x + B u y = C x \begin{cases}\dot{x}=Ax+Bu\\y=Cx\end{cases} {x˙=Ax+Buy=Cx
已知 y y y是可以观测的，要估计状态变量 x x x，如何估计？
设计一个观测器：
{ x ^ ˙ = A x ^ + B u y ^ = C x ^ \begin{cases}\dot{\hat{x}}=A\hat{x}+Bu\\\hat{y}=C\hat{x}\end{cases} {x^˙=Ax^+Buy^=Cx^
在 u u u的情况下，如果知道观测器初始状态 x ( 0 ) x(0) x(0)，就可以估计出 x x x，
但是实际中 x ( 0 ) x(0) x(0)是未知的，如何解决 x ( 0 ) x(0) x(0)未知这个问题？答案是我们可以用y来不断修正，y和x是存在关系的，我们可以观测y，所以我们就可以用y来反推x，
y是否能反推出x呢？不一定，这就涉及到能观性分析，如果不能观，那就不能使用全维观测器了，改用降维观测器。
添加一个控制量 Δ y = y − y ^ \Delta y=y - \hat{y} Δy=y−y^来校正， L L L为增益矩阵：
x ^ ˙ = A x ^ + B u + L ( y − y ^ ) = A x ^ + B u + L ( y − C x ^ ) = ( A − L C ) x ^ + B u + L y \begin{aligned} \dot{\hat{x}}& =A\hat{x}+Bu+L(y-\hat{y}) \\ &=A\hat{x}+Bu+L(y-C\hat{x}) \\ &=(A-LC)\hat{x}+Bu+Ly \end{aligned} x^˙=Ax^+Bu+L(y−y^)=Ax^+Bu+L(y−Cx^)=(A−LC)x^+Bu+Ly
观测器误差为 e = x − x ^ e = x - \hat{x} e=x−x^
x ˙ − x ^ ˙ = A x + B u − [ ( A − L C ) x ^ + B u + L y ] = A x + B u − ( A − L C ) x ^ − B u − L C x = ( A − L C ) x − ( A − L C ) x ^ = ( A − L C ) ( x − x ^ ) \begin{aligned} \dot{x}-\dot{\hat{x}} & =Ax+Bu-[(A-LC) \hat{x}+Bu+L y] \\ & =Ax+Bu-(A-LC) \hat{x}-Bu-LCx \\ & =(A-LC) x-(A-LC) \hat{x} \\ & =(A-LC)(x-\hat{x}) \end{aligned} x˙−x^˙=Ax+Bu−[(A−LC)x^+Bu+Ly]=Ax+Bu−(A−LC)x^−Bu−LCx=(A−LC)x−(A−LC)x^=(A−LC)(x−x^)
要控制 e e e不断减小，这又成了一个控制问题,能控制的只有 L L L矩阵，选择好 L L L使得 A − L C A-LC A−LC全是负特征值，就可以实现 e e e趋于0
状态空间中的控制矩阵A的特征值，就是复数域分析极点配置中的极点。
将状态空间方程转换到复数域
s x = A x + B u y = C x sx = Ax + Bu \\ y=Cx sx=Ax+Buy=Cx
相除再化简：
G ( s ) = y ( s ) u ( s ) = C B s I − A G(s) = \frac{y(s)}{u(s)}= \frac{CB}{sI-A} G(s)=u(s)y(s)=sI−ACB
所以状态空间中的控制矩阵A的特征值，就是复数域分析极点配置中的极点
至于是负特征值才稳定，是因为输出 y ( s ) y(s) y(s)经过反拉氏变换得到的时域输出的幅值A与极点的实部有关，实部类似于 y = e − a t y=e^{-at} y=e−at中a的位置，如果实部为正，则幅值就会随着时间t发散。
这些极点中，主导极点是指在系统的所有闭环极点中，距离虚轴最近且周围没有其他闭环零点的极点，主导极点对系统的动态性能影响最大，起着决定性的主导作用。原因同上，实部的绝对值小，对应的输出分量随时间减小的慢，进而最影响动态性能。
经验上讲，观测器极点应比控制器极点远2~6倍。但如果传感器噪声很大以至于会影响控制品质，则可以相应减小观测器的收敛速度以抑制噪声。
以上面的质量块-弹簧-阻尼器为例，设计一个龙伯格观测器如下：
- 选择极点位置，选实部为-1，则
- 计算出 L − A C L-AC L−AC矩阵，并根据特征值即极点实部计算出L矩阵\
计算增益矩阵L
clear;
m = 1;
k = 1;
b = 0.5;
lambda1 = -1;
lambda2 = -2;A = [0 1;-k/m -b/m
];
B = [0;1/m
];
C = [1 0];
syms l1 l2;
L = [l1; l2];
T = A-L*C;eq1 = lambda1^2 - (T(1,1)+T(2,2))*lambda1 + (T(1,1)*T(2,2)-T(1,2)*T(2,1)) == 0;
eq2 = lambda2^2 - (T(1,1)+T(2,2))*lambda2 + (T(1,1)*T(2,2)-T(1,2)*T(2,1)) == 0;
sol = solve([eq1, eq2], [l1, l2]);
simulink搭建模型
注意将系统初始值设置一下，不然看不出龙伯格观测器的效果
仿真结果：
卡尔曼观测器
卡尔曼滤波分为预测和更新两个部分。
使用卡尔曼要满足：
- 当前时刻状态只和上一时刻状态有关。
- 模型和系统均满足线性关系(满足齐次性和可加性)。
- 引入的噪声符合高斯分布。
还是上面那个系统，是没有引入噪声的情况看似仿真结果还不错，如果引入噪声呢？就会发现结果没有那么尽如人意了。
假设模型噪声误差和y的测量噪声误差都服从高斯分布，我们就可以使用卡尔曼观测器了。
建模和上面差不多，只不过是离散模型:
{ X k = F X k − 1 + B u k − 1 + w k Z k = H X k + w k \begin{cases} X_k = FX_{k-1}+Bu_{k-1}+w_k\\ Z_{k}=HX_k+w_k \end{cases} {Xk=FXk−1+Buk−1+wkZk=HXk+wk
F,B,H：由连续方程中的ABC推导而来
将连续方程转化为离散方程的关键，是找到一个上下帧之间的状态转换方程
可以使用教科书上那种解状态方程离散化，也可以使用欧拉方法近似计算离散化。
以下一共出现多个变量，各自含义如下：
X k X_k Xk：k时刻的状态向量
X k − X_k^- Xk−：根据k-1时刻预测出的k时刻的状态向量，没有考虑w误差
Z m e a s u r e Z^{measure} Zmeasure：k时刻的观测值，没有考虑v误差
X ~ k \tilde{X}_{k} X~k：k时刻的最优估计值，结合了 X k − X_k^- Xk−和 Z m e a s u r e Z^{measure} Zmeasure以及噪声的影响，也是卡尔曼观测器的观测结果
Q Q Q：噪声w的协方差矩阵
R R R：噪声v的协方差矩阵
P k P_k Pk： X k − X ~ k X_k - \tilde{X}_{k} Xk−X~k的协方差矩阵,优化的目标就是计算出一个K矩阵使 P k P_k Pk的对角线元素和最小
P k − P_k^- Pk−： X k − X k − X_k - {X}_{k}^- Xk−Xk−的协方差矩阵
如何预测
模型预测部分我们无法知道噪声影响，所以这部分跟龙伯格差不多，建立一个镜像观测系统：
X k − = F X ~ k − 1 + B u k − 1 X_{k}^{-}=F \tilde{X}_{k-1}+B u_{k-1} Xk−=FX~k−1+Buk−1
其中 X k − X_{k}^{-} Xk−是预测值， X ~ k − 1 \tilde{X}_{k-1} X~k−1是上一帧的最优估计值
当k=0的时候，上一帧的值人为设定，作为初始状态
没有真实值，有误差的真实情况下，确实没办法知道真实值
接下来跟龙伯格不一样的地方来了
用预测值计算出“预测的测量值”：
Z − = H X k − Z^- = HX_k^{-} Z−=HXk−
也可以用测量值来反推“测量到的状态变量”，也可以，反之就是要将模型预测结果和测量结果统一一下
如何更新
更新方程：
X ~ k = X k − + K ( Z m e a s u r e − Z − ) \tilde{X}_k = X_k^- +K(Z^{measure}-Z^-) X~k=Xk−+K(Zmeasure−Z−)
更新方程和龙伯格的预测方程一样，龙伯格观测器和卡尔曼观测器的主要区别也就在这里，对于增益的计算方式不同。
计算K需要考虑噪声的影响
学习龙伯格观测器中的做法，设误差值 E k = X k − X k ~ E_k = X_k - \tilde{X_k} Ek=Xk−Xk~,调整K使得 E k E_k Ek最小，如何调整？跟龙伯格一样，写出 E k E_k Ek的状态转移方程，然后看作一个控制系统来处理
X k ~ = X k − + K ( Z m e a s u r e − Z − ) Z − = H X k − Z m e a s u r e = H X k + v k E k = X k − X k ~ \begin{aligned} \tilde{X_k} & = X_k^- +K(Z^{measure}-Z^-)\\ Z^- &= HX_k^{-}\\ Z^{measure} &= HX_k+v_k\\ E_k & = X_k - \tilde{X_k} \end{aligned} Xk~Z−ZmeasureEk=Xk−+K(Zmeasure−Z−)=HXk−=HXk+vk=Xk−Xk~
将上面几个方程带入到最下面方程里，可以推导出：
E k = ( I − K H ) ( X k − X k − ) − K v k E_k = (I-KH)(X_k - X_k^-)-Kv_k Ek=(I−KH)(Xk−Xk−)−Kvk
E k E_k Ek不仅与 E k − 1 E_{k-1} Ek−1有关，还与噪声 W W W和 V V V有关，要使 E k E_k Ek最小，即使 E k E_k Ek的方差最小，在多维随机变量情况，也就是 E k E_k Ek协方差矩阵的对角线元素都最小。因为各状态变量间不是互不相关的，所以一般做不到，退而求其次使得协方差矩阵的迹最小。
问题变成了，概率论里面的课后习题
( A B ) T = B T A T , ( A + B ) T = A T + B T (AB)^T = B^TA^T,(A+B)^T=A^T+B^T (AB)T=BTAT,(A+B)T=AT+BT
计算E的协方差矩阵：
P = E ( E k E k T ) = E [ ( I − K H ) E k − ( E k − ) T ( I − K H ) T + K v k v k T K T ] = ( I − K H ) E [ E k − 1 E k − 1 T ] ( I − K H ) T + K R K T \begin{aligned} P &= E(E_kE_k^T) \\ & = E\left [ \left ( I-KH \right )E_k^-(E_k^-)^T\left ( I-KH \right )^T + Kv_kv_k^TK^T \right ] \\ & = \left ( I-KH \right ) E\left [ E_{k-1}E_{k-1}^T\right ] \left ( I-KH \right )^T + KRK^T \end{aligned} P=E(EkEkT)=E[(I−KH)Ek−(Ek−)T(I−KH)T+KvkvkTKT]=(I−KH)E[Ek−1Ek−1T](I−KH)T+KRKT
将 E k − = X k − X k − E_k^- = X_k - X_k^- Ek−=Xk−Xk−称为预测误差,其协方差矩阵记作 P k − P_k^- Pk−，则：
P k = ( I − K H ) P k − ( I − K H ) T + K R K T = P k − − K H P k − ( I T − H T K T ) − K R K T = P k − − K H P k − − P k − H T K T + K H P k − H T K T + K R K T \begin{aligned} P_k & = \left ( I-KH \right ) P_k^-\left ( I-KH \right )^T + KRK^T\\ & =P_k^- - KHP_k^-(I^T - H^TK^T) - KRK^T \\ & = P_k^- - KHP_k^- - P_k^-H^TK^T + KHP_k^-H^TK^T + KRK^T \end{aligned} Pk=(I−KH)Pk−(I−KH)T+KRKT=Pk−−KHPk−(IT−HTKT)−KRKT=Pk−−KHPk−−Pk−HTKT+KHPk−HTKT+KRKT
t r ( P k ) tr(P_k) tr(Pk)最小在 d t r ( P k ) K = 0 \frac{dtr(P_k)}{K}=0 Kdtr(Pk)=0时取得，根据这个解出 K = P k − H T ( H P k − H T + R ) − 1 K=P_{k}^{-} H^{T}\left(H P_{k}^{-} H^{T}+R\right)^{-1} K=Pk−HT(HPk−HT+R)−1，带入求得:
P k = ( 1 − K H ) P k − P_k = (1-KH)P_k^- Pk=(1−KH)Pk−
同理联立下面方程
X k = F X k − 1 + B u k − 1 + w k X k − = F X ~ k − 1 + B u k − 1 E k − = X k − X k − P k − = E ( E k − ( E k − ) T ) \begin{aligned} X_k &= FX_{k-1}+Bu_{k-1}+w_k\\ X_{k}^{-}&=F \tilde{X}_{k-1}+B u_{k-1} \\ E_k^- & = X_k - X_k^- \\ P_k^- &= E(E_k^-(E_k^-)^T) \end{aligned} XkXk−Ek−Pk−=FXk−1+Buk−1+wk=FX~k−1+Buk−1=Xk−Xk−=E(Ek−(Ek−)T)
解得:
P k − = F P k − 1 F T + Q P_k^- = FP_{k-1}F^T + Q Pk−=FPk−1FT+Q
这个应该算是预测里面的内容，但是要先引入噪声的计算才能更好理解这个方程
至此，五个公式就已经都得到了
案例仿真如下：
clear;
m = 1;
k = 1;
b = 0.5;A = [0 1;-k/m -b/m
];
B = [0;1/m
];
C = [1 0];N = 10000;
dt = 0.01;
I = [1 0;0 1;
];
F = I + dt*A;
B_sp = dt*B;
u = ones(1,N); %系统输入
Q = diag([0.0025,0.0025]); % 过程噪声W的协方差矩阵
R = 0.0001; % 观测只有一个,噪声也只有一个
W = sqrt(Q)*rand(2,N);
V = sqrt(R)*rand(1,N);
P0 = [1 0;1 1;
]; % 初始协方差估计状态X = [1;1]; % 系统初始状态
x_opt = [0;0]; % 初始估计状态X1_opt_list = zeros(size(u));
X1_predict_list = zeros(size(u));
X1_real_list = zeros(size(u));
X1_opt_errors = zeros(size(u));
X1_pred_errors = zeros(size(u));for epoch = 1 : NXnext = F*X + B_sp*u(epoch)+ W(:,epoch); % 系统真实值ynext = C*X;X = Xnext;X_predict = F*x_opt + B_sp*u(epoch); % 模型预测的值y_measure = ynext + V(epoch);P_predict = F*P0*F' + Q;K = P_predict*C'/(C*P_predict*C'+R);x_opt = X_predict + K * (y_measure - C*X_predict); % 最优化估计的值P0 = (I - K*C)*P_predict;X1_real_list(epoch) = X(1,1);X1_predict_list(epoch) = X_predict(1,1);X1_opt_list(epoch) = x_opt(1,1);X1_opt_errors(epoch) = x_opt(1,1) - X(1,1);X1_pred_errors(epoch) = X_predict(1,1) - X(1,1);
endmean(X1_pred_errors);
mean(X1_opt_errors);
扩展卡尔曼
扩展卡尔曼是将只能用于线性系统的卡尔曼扩展到了非线性系统。
非线性系统需要将系统进行线性化，如何进行线性化可以参考现代控制工程：
找到一个平衡点，然后在该点附近进行线性化。
什么叫在该点附近进行线性化？线性化不是整个系统的线性化吗？
线性化的本质，是拿线性函数来拟合成一个非线性函数
拿线性化最常用的泰勒展开来作为例子，线性化后的系统跟原系统间一定存在误差，如何避免这个误差带来的影响？答案就是，作出取舍，确保常用区间的精度就可以了，例如要计算 e x 10 e^{\frac{x}{10}} e10x，如果已知x服从正态分布 N ( 15 , 4 ) N(15,4) N(15,4)，为了提高计算速度，要使用泰勒展开进行线性化，请问在哪里展开？
下面是选择在x=1处展开展开和在x=10处展开的函数拟合效果：
可以看到x=1处展开函数在x=15处的拟合效果差很多，这就是区别。如果可能，最好的选择是在x=15处展开，效果更好。
同理，对非线性系统的拟合也不可能做到完全消除误差，展开位置的选择，尽量选在稳定平衡点附近，这样系统的大部分时间拟合精度都可以得到保证。
注意，这里的选择展开是要以状态空间变量为坐标系进行参考，而不是以时间进行参考，展开也不是在t= t n t_n tn的时候展开，而是在X= X n X_n Xn的状态展开。
一维时的泰勒展开：
f ( x ) = f ( x 0 ) 0 ! ( x − x 0 ) 0 + f ′ ( x 0 ) 1 ! ( x − x 0 ) 1 + f ′ ′ ( x 0 ) 2 ! ( x − x 0 ) 2 + ⋯ + f ′ n ( x 0 ) n ! ( x − x 0 ) n \begin{aligned} f(x)=\frac{f\left(x_{0}\right)}{0!}\left(x-x_{0}\right)^{0} & +\frac{f^{\prime}\left(x_{0}\right)}{1!}\left(x-x_{0}\right)^{1}+\frac{f^{\prime \prime}\left(x_{0}\right)}{2!}\left(x-x_{0}\right)^{2}+\cdots \\ & +\frac{f^{\prime n}\left(x_{0}\right)}{n!}\left(x-x_{0}\right)^{n} \end{aligned} f(x)=0!f(x0)(x−x0)0+1!f′(x0)(x−x0)1+2!f′′(x0)(x−x0)2+⋯+n!f′n(x0)(x−x0)n
多维时的泰勒展开：
y = f ( x a , x b ) + ∂ f ∂ x 1 ∣ x 1 = x a x 2 = x b ( x 1 − x a ) + ∂ f ∂ x 2 ∣ x 1 = x a x 2 = x b ( x 2 − x b ) + ⋯ y=f\left(x_{a}, x_{b}\right)+\left.\frac{\partial f}{\partial x_{1}}\right|_{\substack{x_{1}=x_{a} \\ x_{2}=x_{b}}}\left(x_{1}-x_{a}\right)+\left.\frac{\partial f}{\partial x_{2}}\right|_{\substack{x_{1}=x_{a} \\ x_{2}=x_{b}}}\left(x_{2}-x_{b}\right)+\cdots y=f(xa,xb)+∂x1∂f x1=xax2=xb(x1−xa)+∂x2∂f x1=xax2=xb(x2−xb)+⋯
线性化只展开到 f ‵ ( x ) ( x − x 0 ) f‵(x)(x-x_0) f‵(x)(x−x0)，后面都高阶量都不要， ∂ f ∂ x 1 ∣ x 1 = x a x 2 = x b \left.\frac{\partial f}{\partial x_{1}}\right|_{\substack{x_{1}=x_{a} \\ x_{2}=x_{b}}} ∂x1∂f x1=xax2=xb表示二维函数的导数，即雅各比矩阵
{ x ˙ 1 = f 1 ( a , b ) + ∂ f 1 ∂ x 1 ∣ x 1 = x a x 2 = x b ( x 1 − a ) + ∂ f 1 ∂ x 2 ∣ x 1 = x a x 2 = x b ( x 2 − b ) x ˙ 2 = f 2 ( a , b ) + ∂ f 2 ∂ x 1 ∣ x 1 = x a x 2 = x b ( x 1 − a ) + ∂ f 2 ∂ x 2 ∣ x 1 = x a x 2 = x b ( x 2 − b ) \left\{\begin{array}{l} \dot{x}_{1}=f_{1}(a, b)+\left.\frac{\partial f_{1}}{\partial x_{1}}\right|_{\substack{x_{1}=x_{a} \\ x_{2}=x_{b}}}\left(x_{1}-a\right)+\left.\frac{\partial f_{1}}{\partial x_{2}}\right|_{\substack{x_{1}=x_{a} \\ x_{2}=x_{b}}}\left(x_{2}-b\right) \\ \dot{x}_{2}=f_{2}(a, b)+\left.\frac{\partial f_{2}}{\partial x_{1}}\right|_{\substack{x_{1}=x_{a} \\ x_{2}=x_{b}}}\left(x_{1}-a\right)+\left.\frac{\partial f_{2}}{\partial x_{2}}\right|_{\substack{x_{1}=x_{a} \\ x_{2}=x_{b}}}\left(x_{2}-b\right) \end{array}\right. ⎩ ⎨ ⎧x˙1=f1(a,b)+∂x1∂f1 x1=xax2=xb(x1−a)+∂x2∂f1 x1=xax2=xb(x2−b)x˙2=f2(a,b)+∂x1∂f2 x1=xax2=xb(x1−a)+∂x2∂f2 x1=xax2=xb(x2−b)
平衡点如何找？
找导数为0的点，即 f ‵ ( a , b ) = 0 f‵(a,b)=0 f‵(a,b)=0的点。
做完这些，根据这个调整卡尔曼观测器的方程，就是扩展卡尔曼：
由于模型预测下一时刻状态时直接使用非线性模型即可，没必要用拟合的线性模型，噪声无法得知，所以默认为0
X k − = f ( X ~ k − 1 , u k − 1 , w = 0 ) Z − = h ( X k − , v = 0 ) \begin{aligned} X_{k}^{-} & = f(\tilde{X}_{k-1},u_{k-1},w=0) \\ Z^- &= h(X_{k}^-,v=0) \end{aligned} Xk−Z−=f(X~k−1,uk−1,w=0)=h(Xk−,v=0)
因为线性化之前的噪声经过线性化协方差矩阵发生了变化，变成了 W Q W T WQW^T WQWT和 V R V T VRV^T VRVT，涉及这俩变量的公式修改：
W和V是f和h对w和v求导的雅各比矩阵，一般是单位矩阵 I I I
P k − = F P k − 1 F T + W Q W T K = P k − H T ( H P k − H T + V R V ) − 1 \begin{aligned} P_k^- & =FP_{k-1}F^T+WQW^T \\ K & =P_{k}^{-} H^{T}\left(H P_{k}^{-} H^{T}+VRV\right)^{-1} \end{aligned} Pk−K=FPk−1FT+WQWT=Pk−HT(HPk−HT+VRV)−1
P k P_k Pk的计算函数不需要改。