本節(jié)默認(rèn)你能夠完整推導(dǎo)標(biāo)準(zhǔn)卡爾曼濾波，將會(huì)簡(jiǎn)化一些推導(dǎo)的描述。如果你還不會(huì)完整推導(dǎo)標(biāo)準(zhǔn)卡爾曼濾波，請(qǐng)先從 【卡爾曼濾波理論推導(dǎo)與實(shí)踐】系列開始看起。

---

非線性系統(tǒng)

擴(kuò)展卡爾曼濾波解決的是非線性系統(tǒng)，其非線性狀態(tài)空間方程為：
$$\{\begin{array}{l}\overrightarrow{z_{k模}}=\mathrm{f}(\overrightarrow{z_{k?1模}},F_{k?1})\\ \overrightarrow{y_{k測(cè)}}=\mathrm{h}(\overrightarrow{z_{k測(cè)}})\end{array}$$
$\mathrm{f},\mathrm{h}$函數(shù)是向量函數(shù)，有多個(gè)輸出$\left[\begin{array}{c}{f}_1\\ ...\\ f_n\end{array}\right]$，$n$是$\vec{z}$的維數(shù)，比如追蹤平面圖像物體坐標(biāo)和速度，$\vec{z}=\left[\begin{array}{c}x\\ y\\ v_x\\ v_y\end{array}\right]$。
其非線性真實(shí)系統(tǒng)模型為：
$$\{\begin{array}{l}\overrightarrow{z_{k真}}=\mathrm{f}(\overrightarrow{z_{k?1真}},F_{k?1},\overrightarrow{w_{k?1}})\\ \overrightarrow{y_{k測(cè)}}=\mathrm{h}(\overrightarrow{z_{k真}},\overrightarrow{v_k})\end{array}$$

因?yàn)榉蔷€性函數(shù)的期望不能用協(xié)方差矩陣描述，無(wú)法用卡爾曼濾波的套路進(jìn)行推導(dǎo)，所以要進(jìn)行線性化近似，這就是擴(kuò)展卡爾曼濾波與標(biāo)準(zhǔn)卡爾曼濾波的唯一區(qū)別。

泰勒展開

擴(kuò)展卡爾曼濾波使用泰勒展開進(jìn)行線性化近似，擴(kuò)展卡爾曼濾波是一個(gè)二元向量輸入多輸出系統(tǒng)，比如輸入為$[\overrightarrow{z_{k?1模}},\overrightarrow{w_{k?1}}]$，這有兩列向量；輸出為$\overrightarrow{z_{k真}}$這個(gè)向量。
這里有兩個(gè)公式要提前知道：

• 單輸出的二元函數(shù)在$(x_0,y_0)$點(diǎn)的泰勒展開公式：
  $$f(x,y)=f(x_0,x_0)+\frac{df}{dx}{|}_{x_0,y_0}?(x?x_0)++\frac{df}{dy}{|}_{x_0,y_0}?(y?y_0)$$
• 多輸出函數(shù)對(duì)輸入向量求導(dǎo)公式：
  $$\frac{d\vec{f}(\vec{x})}{d\vec{x}}=\left[\begin{array}{cccc}\frac{d{f}_1}{d{x}_1}& \frac{d{f}_1}{d{x}_2}& ?& \frac{d{f}_1}{d{x}_m}\\ \frac{d{f}_2}{d{x}_1}& \frac{d{f}_2}{d{x}_2}& ?& \frac{d{f}_2}{d{x}_m}\\ ?& ?& ?& ?\\ \frac{d{f}_n}{d{x}_1}& \frac{d{f}_n}{d{x}_2}& ?& \frac{d{f}_n}{d{x}_m}\end{array}\right]=雅可比矩陣$$

接下來(lái)進(jìn)行泰勒展開：
記$\mathrm{f}$對(duì)$\vec{z}$求導(dǎo)的雅可比矩陣為$\mathrm{A}$，$\mathrm{f}$對(duì)$\vec{w}$求導(dǎo)的雅可比矩陣為$\mathrm{W}$，$\mathrm{h}$對(duì)$\vec{z}$求導(dǎo)的雅可比矩陣為$\mathrm{H}$，$\mathrm{h}$對(duì)$\vec{v}$求導(dǎo)的雅可比矩陣為$\mathrm{V}$。
注意：

• $F_{k?1}$是給定值而不是變量，不參與泰勒展開
• 噪聲的期望（平均值）是零，所以泰勒展開點(diǎn)的噪聲值為零

可以在任意已知點(diǎn)（比如模型值和上輪估計(jì)值都是已知的）展開，在哪個(gè)點(diǎn)展開都是為了方便推導(dǎo)。
狀態(tài)轉(zhuǎn)移方程分別在$\overrightarrow{z_{k?1估}},\overrightarrow{z_{k模}}$處泰勒展開：
$$\{\begin{array}{l}\overrightarrow{z_{k模}}=f(\overrightarrow{z_{k?1估}},F_{k?1})+\mathrm{A}?(\overrightarrow{z_{k?1模}}?\overrightarrow{z_{k?1估}})\\ \overrightarrow{y_{k測(cè)}}=h(\overrightarrow{z_{k模}})+\mathrm{H}?(\overrightarrow{z_{k測(cè)}}?\overrightarrow{z_{k模}})\end{array}$$
真實(shí)系統(tǒng)模型分別在$(\overrightarrow{z_{k?1估}},\vec{0}),(\overrightarrow{z_{k模}},\vec{0})$泰勒展開：
$$\{\begin{array}{l}\overrightarrow{z_{k真}}=f(\overrightarrow{z_{k?1估}},F_{k?1},\vec{0})+\mathrm{A}?(\overrightarrow{z_{k?1真}}?\overrightarrow{z_{k?1估}})+\mathrm{W}?(\overrightarrow{w_{k?1}}?\vec{0})\\ \overrightarrow{y_{k測(cè)}}=h(\overrightarrow{z_{k模}},\vec{0})+\mathrm{H}?(\overrightarrow{z_{k真}}?\overrightarrow{z_{k模}})+\mathrm{V}?(\overrightarrow{v_k}?\vec{0})\end{array}$$

卡爾曼濾波

線性化后，接下來(lái)的推導(dǎo)過程和標(biāo)準(zhǔn)卡爾曼濾波不能說一摸一樣，只能說九分甚至十分地相似。
這里再放一遍卡爾曼濾波核心思想：

用測(cè)量值修正系統(tǒng)模型值，得到當(dāng)前輪次的最優(yōu)估計(jì)值。再將該最優(yōu)估計(jì)值當(dāng)作下一輪的系統(tǒng)模型輸入，繼續(xù)進(jìn)行下一輪的最優(yōu)估計(jì)，如此迭代進(jìn)行。

要最優(yōu)化估計(jì)值，首先把估計(jì)值表達(dá)式寫出來(lái)：
$$\begin{array}{rlr}\overrightarrow{z_{k估}}& =\overrightarrow{z_{k模}}+{\mathrm{G}}_{\mathrm{k}}?(\overrightarrow{z_{k測(cè)}}?\overrightarrow{z_{k模}})& \end{array}$$
對(duì)$\mathrm{G}$矩陣進(jìn)行一個(gè)變換，變出一個(gè)$\overrightarrow{y_{k測(cè)}}$出來(lái)，方便推導(dǎo)，令$\mathrm{G}=\mathrm{K}?\mathrm{H}$，于是估計(jì)值表達(dá)式變?yōu)?#xff1a;
$$\begin{array}{rl}\overrightarrow{z_{k估}}& =\overrightarrow{z_{k模}}+{\mathrm{K}}_{\mathrm{k}}?\mathrm{H}?(\overrightarrow{z_{k測(cè)}}?\overrightarrow{z_{k模}})\\ & =\overrightarrow{z_{k模}}+{\mathrm{K}}_{\mathrm{k}}?(\overrightarrow{y_{k測(cè)}}?\mathrm{H}?\overrightarrow{z_{k模}})\end{array}$$

卡爾曼增益

$\overrightarrow{z_{k估}}$表達(dá)式右側(cè)除了${\mathrm{K}}_{\mathrm{k}}$不知道，其他都是已知數(shù)，繼續(xù)推導(dǎo)${\mathrm{K}}_{\mathrm{k}}$吧。那么就是推導(dǎo)最優(yōu)估計(jì)下的${\mathrm{K}}_{\mathrm{k}}$，就是使得估計(jì)誤差協(xié)方差矩陣的跡最小，先把估計(jì)誤差協(xié)方差矩陣寫出來(lái)：
$$\begin{array}{rl}{\mathrm{P}}_{\mathrm{k}\mathrm{估}}& =E[\overrightarrow{e_{k估}}{\overrightarrow{e_{k估}}}^T]\\ & =E[(\overrightarrow{z_{k真}}?\overrightarrow{z_{k估}})?(\overrightarrow{z_{k真}}?\overrightarrow{z_{k估}}{)}^T]\end{array}$$

展開一下$\overrightarrow{z_{k真}}?\overrightarrow{z_{k估}}$:
$$\begin{array}{rl}\overrightarrow{z_{k真}}?\overrightarrow{z_{k估}}& =\overrightarrow{z_{k真}}?\overrightarrow{z_{k模}}?{\mathrm{K}}_{\mathrm{k}}?(\overrightarrow{y_{k測(cè)}}?\mathrm{h}(\overrightarrow{z_{k模}}))\\ & =\overrightarrow{e_{k模}}?{\mathrm{K}}_{\mathrm{k}}?(h(\overrightarrow{z_{k模}},0)+\mathrm{H}?(\overrightarrow{z_{k真}}?\overrightarrow{z_{k模}})+\mathrm{V}?\overrightarrow{v_k}?h(\overrightarrow{z_{k模}}))\\ & =\overrightarrow{e_{k模}}?{\mathrm{K}}_{\mathrm{k}}?(\mathrm{H}?\overrightarrow{e_{k模}}+\mathrm{V}?\overrightarrow{v_k})\\ & =(\mathrm{I}?{\mathrm{K}}_{\mathrm{k}}\mathrm{H})\overrightarrow{e_{k模}}?{\mathrm{K}}_{\mathrm{k}}\mathrm{V}?\overrightarrow{v_k}\end{array}$$

${\mathrm{P}}_{\mathrm{k}\mathrm{估}}$可以繼續(xù)展開了：
$$\begin{array}{rl}{\mathrm{P}}_{\mathrm{k}\mathrm{估}}& =E[((\mathrm{I}?{\mathrm{K}}_{\mathrm{k}}\mathrm{H})\overrightarrow{e_{k模}}?{\mathrm{K}}_{\mathrm{k}}\mathrm{V}?\overrightarrow{v_k})?((\mathrm{I}?{\mathrm{K}}_{\mathrm{k}}\mathrm{H})\overrightarrow{e_{k模}}?{\mathrm{K}}_{\mathrm{k}}\mathrm{V}?\overrightarrow{v_k}{)}^T]\\ & =E[((\mathrm{I}?{\mathrm{K}}_{\mathrm{k}}\mathrm{H})\overrightarrow{e_{k模}}?{\mathrm{K}}_{\mathrm{k}}\mathrm{V}?\overrightarrow{v_k})?({\overrightarrow{e_{k模}}}^T(\mathrm{I}?{\mathrm{K}}_{\mathrm{k}}\mathrm{H}{)}^T?{\overrightarrow{v_k}}^T{\mathrm{V}}^T{{\mathrm{K}}_{\mathrm{k}}}^T)]\end{array}$$
接下來(lái)將括號(hào)乘開。這里有兩點(diǎn)有助于化簡(jiǎn)：噪聲$\overrightarrow{v_k}$的期望是零，又與$\overrightarrow{e_{k模}}$相互獨(dú)立，所以$\overrightarrow{v_k}$和$\overrightarrow{e_{k模}}$搭邊的項(xiàng)都是零。
$$\begin{array}{rl}{\mathrm{P}}_{\mathrm{k}\mathrm{估}}& =E[(\mathrm{I}?{\mathrm{K}}_{\mathrm{k}}\mathrm{H})\overrightarrow{e_{k模}}{\overrightarrow{e_{k模}}}^T(\mathrm{I}?{\mathrm{K}}_{\mathrm{k}}\mathrm{H}{)}^T+{\mathrm{K}}_{\mathrm{k}}\mathrm{V}?\overrightarrow{v_k}{\overrightarrow{v_k}}^T{\mathrm{V}}^T{{\mathrm{K}}_{\mathrm{k}}}^T]\\ & =(\mathrm{I}?{\mathrm{K}}_{\mathrm{k}}\mathrm{H}){\mathrm{P}}_{\mathrm{k}\mathrm{模}}(\mathrm{I}?{\mathrm{K}}_{\mathrm{k}}\mathrm{H}{)}^T+{\mathrm{K}}_{\mathrm{k}}\mathrm{VR}{\mathrm{V}}^T{{\mathrm{K}}_{\mathrm{k}}}^T\end{array}$$
${\mathrm{P}}_{\mathrm{k}\mathrm{估}}$的表達(dá)式也出來(lái)了，${\mathrm{K}}_{\mathrm{k}}$是自變量，想讓$tr({\mathrm{P}}_{\mathrm{k}\mathrm{估}})$最小，即最優(yōu)估計(jì)。由于$tr({\mathrm{P}}_{\mathrm{k}\mathrm{估}})$是二次函數(shù)，開口向上，極值點(diǎn)必定是極小值點(diǎn)，于是接下來(lái)準(zhǔn)備$\frac{tr({\mathrm{P}}_{\mathrm{k}\mathrm{估}})}{d{\mathrm{K}}_{\mathrm{k}}}=0$。在算極值點(diǎn)前，有個(gè)矩陣求導(dǎo)公式需要提前知道：
$$\frac{dtr(AB{A}^T)}{d\mathrm{A}}=2AB$$
這個(gè)求導(dǎo)公式結(jié)合鏈?zhǔn)角髮?dǎo)法則，就可以進(jìn)行求極值點(diǎn)了，注意$\mathrm{VR}{\mathrm{V}}^T$看作一個(gè)整體：
$$\begin{array}{rl}\frac{d{\mathrm{P}}_{\mathrm{k}\mathrm{估}}}{d{\mathrm{K}}_{\mathrm{k}}}& =2(\mathrm{I}?{\mathrm{K}}_{\mathrm{k}}\mathrm{H}){\mathrm{P}}_{\mathrm{k}\mathrm{模}}(?{\mathrm{H}}^T)+2{\mathrm{K}}_{\mathrm{k}}\mathrm{VR}{\mathrm{V}}^T\\ & =0\end{array}$$

${\mathrm{K}}_{\mathrm{k}}$這不就出來(lái)了，和標(biāo)準(zhǔn)卡爾曼濾波里的${\mathrm{K}}_{\mathrm{k}}$挺像的：
$${\mathrm{K}}_{\mathrm{k}}=\frac{{\mathrm{P}}_{\mathrm{k}\mathrm{模}}{\mathrm{H}}^T}{\mathrm{H}{\mathrm{P}}_{\mathrm{k}\mathrm{模}}{\mathrm{H}}^T+\mathrm{VR}{\mathrm{V}}^T}$$

模型誤差協(xié)方差矩陣

和標(biāo)準(zhǔn)卡爾曼濾波的推導(dǎo)套路一樣，繼續(xù)推導(dǎo)${\mathrm{P}}_{\mathrm{k}\mathrm{模}}$。
先把表達(dá)式搞出來(lái)：
$$\begin{array}{rl}{\mathrm{P}}_{\mathrm{k}\mathrm{模}}& =E[\overrightarrow{e_{k模}}{\overrightarrow{e_{k模}}}^T]\\ & =E[(\overrightarrow{z_{k真}}?\overrightarrow{z_{k模}})?(\overrightarrow{z_{k真}}?\overrightarrow{z_{k模}}{)}^T]\end{array}$$

展開一下$\overrightarrow{z_{k真}}?\overrightarrow{z_{k模}}$：
$$\begin{array}{rl}\overrightarrow{z_{k真}}?\overrightarrow{z_{k模}}& =f(\overrightarrow{z_{k?1估}},F_{k?1},0)+\mathrm{A}?(\overrightarrow{z_{k?1真}}?\overrightarrow{z_{k?1估}})+\mathrm{W}?\overrightarrow{w_{k?1}}?(f(\overrightarrow{z_{k?1估}},F_{k?1})+\mathrm{A}?(\overrightarrow{z_{k?1模}}?\overrightarrow{z_{k?1估}}))\\ & =\mathrm{A}(\overrightarrow{z_{k?1真}}?\overrightarrow{z_{k?1估}})+\mathrm{W}?\overrightarrow{w_{k?1}}\\ & =\mathrm{A}\overrightarrow{e_{k估}}+\mathrm{W}\overrightarrow{w_{k?1}}\end{array}$$

${\mathrm{P}}_{\mathrm{k}\mathrm{模}}$繼續(xù)展開，展開后的表達(dá)式和標(biāo)準(zhǔn)卡爾曼濾波里的${\mathrm{P}}_{\mathrm{k}\mathrm{模}}$也挺像的：
$$\begin{array}{rl}{\mathrm{P}}_{\mathrm{k}\mathrm{模}}& =E[(\mathrm{A}\overrightarrow{e_{k?1估}}+\mathrm{W}\overrightarrow{w_{k?1}})?(\mathrm{A}\overrightarrow{e_{k?1估}}+\mathrm{W}\overrightarrow{w_{k?1}}{)}^T]\\ & =E[(\mathrm{A}\overrightarrow{e_{k?1估}}+\mathrm{W}\overrightarrow{w_{k?1}})?({\overrightarrow{e_{k?1估}}}^T{\mathrm{A}}^T+{\overrightarrow{w_{k?1}}}^T{\mathrm{W}}^T)]\\ & =E[\mathrm{A}\overrightarrow{e_{k?1估}}{\overrightarrow{e_{k?1估}}}^T{\mathrm{A}}^T+\mathrm{W}\overrightarrow{w_{k?1}}{\overrightarrow{w_{k?1}}}^T{\mathrm{W}}^T]\\ & =\mathrm{A}E[\overrightarrow{e_{k?1估}}{\overrightarrow{e_{k?1估}}}^T]{\mathrm{A}}^T+\mathrm{W}E[\overrightarrow{w_{k?1}}{\overrightarrow{w_{k?1}}}^T]{\mathrm{W}}^T\\ & =\mathrm{A}{\mathrm{P}}_{\mathrm{k}?1\mathrm{估}}{\mathrm{A}}^T+\mathrm{WQ}{\mathrm{W}}^T\end{array}$$

${\mathrm{P}}_{\mathrm{k}?1\mathrm{估}}$表達(dá)式在上面推導(dǎo)過了，只不過$k$換成$k?1$：
$$表達(dá)式：{\mathrm{P}}_{\mathrm{k}?1\mathrm{估}}=(\mathrm{I}?{\mathrm{K}}_{\mathrm{k}?1}\mathrm{H}){\mathrm{P}}_{\mathrm{k}?1\mathrm{模}}(\mathrm{I}?{\mathrm{H}}^T{{\mathrm{K}}_{\mathrm{k}?1}}^T)+{\mathrm{K}}_{\mathrm{k}?1}\mathrm{VR}{\mathrm{V}}^T{{\mathrm{K}}_{\mathrm{k}?1}}^T，{\mathrm{K}}_{\mathrm{k}?1}=\frac{{\mathrm{P}}_{\mathrm{k}?1\mathrm{模}}{\mathrm{H}}^T}{\mathrm{H}{\mathrm{P}}_{\mathrm{k}?1\mathrm{模}}{\mathrm{H}}^T+\mathrm{VR}{\mathrm{V}}^T}$$
把${\mathrm{K}}_{\mathrm{k}?1}$代到${\mathrm{P}}_{\mathrm{k}?1\mathrm{估}}$表達(dá)式里就得到${\mathrm{P}}_{\mathrm{k}?1\mathrm{估}}$的具體值，你完全可以將${\mathrm{K}}_{\mathrm{k}?1}$直接無(wú)腦代入${\mathrm{P}}_{\mathrm{k}?1\mathrm{估}}$的表達(dá)式，畢竟${\mathrm{P}}_{\mathrm{k}?1\mathrm{估}}$等式右邊全是上輪迭代得到的已知數(shù)，但是${\mathrm{P}}_{\mathrm{k}?1\mathrm{估}}$可以進(jìn)行化簡(jiǎn)減少計(jì)算量：
$$\begin{array}{rl}{\mathrm{P}}_{\mathrm{k}?1\mathrm{估}}& =({\mathrm{P}}_{\mathrm{k}?1\mathrm{模}}?{\mathrm{K}}_{\mathrm{k}?1}\mathrm{H}{\mathrm{P}}_{\mathrm{k}?1\mathrm{模}}?{\mathrm{P}}_{\mathrm{k}?1\mathrm{模}}{\mathrm{H}}^T{{\mathrm{K}}_{\mathrm{k}?1}}^T+{\mathrm{K}}_{\mathrm{k}?1}\mathrm{H}{\mathrm{P}}_{\mathrm{k}?1\mathrm{模}}{\mathrm{H}}^T{{\mathrm{K}}_{\mathrm{k}?1}}^T)+{\mathrm{K}}_{\mathrm{k}?1}\mathrm{VR}{\mathrm{V}}^T{{\mathrm{K}}_{\mathrm{k}?1}}^T\\ & =({\mathrm{P}}_{\mathrm{k}?1\mathrm{模}}?{\mathrm{K}}_{\mathrm{k}?1}\mathrm{H}{\mathrm{P}}_{\mathrm{k}?1\mathrm{模}}?{\mathrm{P}}_{\mathrm{k}?1\mathrm{模}}{\mathrm{H}}^T{{\mathrm{K}}_{\mathrm{k}?1}}^T)+{\mathrm{K}}_{\mathrm{k}?1}(\mathrm{H}{\mathrm{P}}_{\mathrm{k}?1\mathrm{模}}{\mathrm{H}}^T+\mathrm{VR}{\mathrm{V}}^T){{\mathrm{K}}_{\mathrm{k}?1}}^T\\ & =({\mathrm{P}}_{\mathrm{k}?1\mathrm{模}}?{\mathrm{K}}_{\mathrm{k}?1}\mathrm{H}{\mathrm{P}}_{\mathrm{k}?1\mathrm{模}}?{\mathrm{P}}_{\mathrm{k}?1\mathrm{模}}{\mathrm{H}}^T{{\mathrm{K}}_{\mathrm{k}?1}}^T)+{\mathrm{P}}_{\mathrm{k}?1\mathrm{模}}{\mathrm{H}}^T{{\mathrm{K}}_{\mathrm{k}?1}}^T\\ & ={\mathrm{P}}_{\mathrm{k}?1\mathrm{模}}?{\mathrm{K}}_{\mathrm{k}?1}\mathrm{H}{\mathrm{P}}_{\mathrm{k}?1\mathrm{模}}\\ & =(\mathrm{I}?{\mathrm{K}}_{\mathrm{k}?1}\mathrm{H}){\mathrm{P}}_{\mathrm{k}?1\mathrm{模}}\end{array}$$
又是和標(biāo)準(zhǔn)卡爾曼濾波里的${\mathrm{P}}_{\mathrm{k}?1\mathrm{估}}$長(zhǎng)得挺像的。
這樣擴(kuò)展卡爾曼濾波的公式全推導(dǎo)完了。

公式總結(jié)

1. $\overrightarrow{z_{k模}}=\mathrm{f}(\overrightarrow{z_{k?1模}},F_{k?1})$
2. ${\mathrm{P}}_{\mathrm{k}\mathrm{模}}=\mathrm{A}{\mathrm{P}}_{\mathrm{k}?1\mathrm{估}}{\mathrm{A}}^T+\mathrm{WQ}{\mathrm{W}}^T$
3. ${\mathrm{K}}_{\mathrm{k}}=\frac{{\mathrm{P}}_{\mathrm{k}\mathrm{模}}{\mathrm{H}}^T}{\mathrm{H}{\mathrm{P}}_{\mathrm{k}\mathrm{模}}{\mathrm{H}}^T+\mathrm{VR}{\mathrm{V}}^T}$
4. $\overrightarrow{z_{k估}}=\overrightarrow{z_{k模}}+{\mathrm{K}}_{\mathrm{k}}?(\overrightarrow{y_{k測(cè)}}?\mathrm{h}(\overrightarrow{z_{k模}}))$
5. ${\mathrm{P}}_{\mathrm{k}\mathrm{估}}=(\mathrm{I}?{\mathrm{K}}_{\mathrm{k}}\mathrm{H}){\mathrm{P}}_{\mathrm{k}\mathrm{模}}$

大致迭代過程和標(biāo)準(zhǔn)卡爾曼濾波一樣，這五個(gè)公式按照順序遍歷計(jì)算。要注意的是標(biāo)準(zhǔn)卡爾曼濾波的$\mathrm{A},\mathrm{V},\mathrm{W}$都是固定的矩陣，而擴(kuò)展卡爾曼濾波中的$\mathrm{A},\mathrm{V},\mathrm{W}$是雅可比矩陣，每次迭代過程中都需要更新計(jì)算一次。