发明名称 基于有限差分扩展卡尔曼算法的锂电池荷电状态估计方法
摘要 本发明公开了一种锂电池荷电状态的估计方法,该方法包括以下步骤:步骤一、给定初始值,对各协方差进行Cholesky分解;步骤二、状态的一步预测;步骤三、协方差的一步预测;步骤四、滤波增益;步骤五、更新状态的最优值;步骤六、滤波协方差更新。与现有技术相比,本发明的精度高于泰勒级数的一阶展开,而且,充分利用了由模型线性化产生的有效误差信息,对模型参数变动具有较强的鲁棒性。
申请公布号 CN103116136B 申请公布日期 2015.07.15
申请号 CN201310021706.1 申请日期 2013.01.21
申请人 天津大学 发明人 程泽;刘艳莉;张玉晖;戴胜;张秋艳
分类号 G01R31/36(2006.01)I 主分类号 G01R31/36(2006.01)I
代理机构 天津市北洋有限责任专利代理事务所 12201 代理人 李素兰
主权项 一种基于有限差分扩展卡尔曼算法的锂电池荷电状态估计方法,其特征在于,该方法包括以下步骤:通过建立锂电池电量的数学模型,得到电池系统荷电状态方程x及荷电状态观测方程y如下:x<sub>k+1</sub>=f(x<sub>k</sub>,u<sub>k</sub>,w<sub>k</sub>)y<sub>k</sub>=g(x<sub>k</sub>,u<sub>k</sub>,v<sub>k</sub>),其中,x<sub>k</sub>∈R<sup>n</sup>和y<sub>k</sub>∈R<sup>m</sup>分别为k时刻的系统n维状态向量和m维观测向量,f:R<sup>n</sup>→R<sup>n</sup>和g:R<sup>n</sup>→R<sup>m</sup>分别为系统状态函数和观测函数,w<sub>k</sub>和v<sub>k</sub>是互不相关的高斯白噪声,且假设噪声有如下统计学特性:<maths num="0001" id="cmaths0001"><math><![CDATA[<mfenced open='{' close=''><mtable><mtr><mtd><mi>E</mi><mo>[</mo><msub><mi>w</mi><mi>k</mi></msub><mo>]</mo><mo>=</mo><msub><mi>w</mi><mi>k</mi></msub><mo>,</mo><mi>Cov</mi><mo>[</mo><msub><mi>w</mi><mi>k</mi></msub><mo>,</mo><msubsup><mi>w</mi><mi>j</mi><mi>T</mi></msubsup><mo>]</mo><mo>=</mo><msub><mi>Q</mi><mi>k</mi></msub><msub><mi>&delta;</mi><mi>kj</mi></msub><mo>,</mo></mtd></mtr><mtr><mtd><mi>E</mi><mo>[</mo><msub><mi>v</mi><mi>k</mi></msub><mo>]</mo><mo>=</mo><msub><mi>v</mi><mi>k</mi></msub><mo>,</mo><mi>Cov</mi><mo>[</mo><msub><mi>v</mi><mi>k</mi></msub><mo>,</mo><msubsup><mi>v</mi><mi>j</mi><mi>T</mi></msubsup><mo>]</mo><mo>=</mo><msub><mi>R</mi><mi>k</mi></msub><msub><mi>&delta;</mi><mi>kj</mi></msub><mo>,</mo></mtd></mtr><mtr><mtd><mi>Cov</mi><mo>[</mo><msub><mi>w</mi><mi>k</mi></msub><mo>,</mo><msub><mi>v</mi><mi>k</mi></msub><mo>]</mo><mo>=</mo><mn>0</mn><mo>,</mo></mtd></mtr></mtable></mfenced>]]></math><img file="FDA0000712394030000011.GIF" wi="717" he="283" /></maths>其中:Q<sub>k</sub>,R<sub>k</sub>都为正定对称阵;δ<sub>kj</sub>为Kronecker‑δ函数;步骤一、给定初始值,初始化<img file="FDA00007123940300000113.GIF" wi="805" he="93" />对过程噪声方差Q<sub>k</sub>、观测噪声方差R<sub>k</sub>、验前状态方差<img file="FDA00007123940300000114.GIF" wi="104" he="90" />验后状态方差<img file="FDA00007123940300000115.GIF" wi="72" he="74" />进行Cholesky分解,有<maths num="0002" id="cmaths0002"><math><![CDATA[<mrow><mi>Q</mi><mo>=</mo><msub><mi>S</mi><mi>w</mi></msub><msubsup><mi>S</mi><mi>w</mi><mi>T</mi></msubsup><mo>,</mo></mrow>]]></math><img file="FDA0000712394030000012.GIF" wi="230" he="93" /></maths><maths num="0003" id="cmaths0003"><math><![CDATA[<mrow><mi>R</mi><mo>=</mo><msub><mi>S</mi><mi>v</mi></msub><msubsup><mi>S</mi><mi>v</mi><mi>T</mi></msubsup><mo>,</mo></mrow>]]></math><img file="FDA0000712394030000013.GIF" wi="219" he="93" /></maths><maths num="0004" id="cmaths0004"><math><![CDATA[<mrow><mover><mi>P</mi><mo>&OverBar;</mo></mover><mo>=</mo><msub><mover><mi>S</mi><mo>&OverBar;</mo></mover><mi>x</mi></msub><msubsup><mover><mi>S</mi><mo>&OverBar;</mo></mover><mi>x</mi><mi>T</mi></msubsup><mo>,</mo></mrow>]]></math><img file="FDA0000712394030000014.GIF" wi="236" he="93" /></maths><maths num="0005" id="cmaths0005"><math><![CDATA[<mrow><mover><mi>P</mi><mo>^</mo></mover><mo>=</mo><msub><mover><mi>S</mi><mo>^</mo></mover><mi>x</mi></msub><msubsup><mover><mi>S</mi><mo>^</mo></mover><mi>x</mi><mi>T</mi></msubsup><mo>,</mo></mrow>]]></math><img file="FDA0000712394030000015.GIF" wi="232" he="101" /></maths>S<sub>w</sub>为过程噪声方差矩阵Q<sub>k</sub>Cholesky分解下的三角矩阵,S<sub>v</sub>为观测噪声方差矩阵R<sub>k</sub>Cholesky分解的下三角矩阵,<img file="FDA00007123940300000111.GIF" wi="70" he="73" />为验前状态方差矩阵<img file="FDA00007123940300000112.GIF" wi="83" he="87" />Cholesky分解的下三角矩阵,<img file="FDA0000712394030000016.GIF" wi="65" he="85" />为验后状态方差矩阵<img file="FDA0000712394030000017.GIF" wi="71" he="83" />Cholesky分解的下三角矩阵;利用一阶中心差分计算非线性函数偏导数,即<maths num="0006" id="cmaths0006"><math><![CDATA[<mrow><msub><mi>F</mi><mi>x</mi></msub><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>=</mo><mrow><mo>(</mo><mi>f</mi><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi></msub><mo>+</mo><mi>&Delta;</mi><msub><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi></msub><mo>,</mo><msub><mi>u</mi><mi>k</mi></msub><mo>,</mo><msub><mi>w</mi><mi>k</mi></msub><mo>)</mo></mrow><mo>-</mo><mi>f</mi><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi></msub><mo>-</mo><mi>&Delta;</mi><msub><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi></msub><mo>,</mo><msub><mi>u</mi><mi>k</mi></msub><mo>,</mo><msub><mi>w</mi><mi>k</mi></msub><mo>)</mo></mrow><mo>)</mo></mrow><mo>/</mo><mn>2</mn><mi>&Delta;</mi><msub><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi></msub><mo>,</mo></mrow>]]></math><img file="FDA0000712394030000018.GIF" wi="1179" he="104" /></maths>令<img file="FDA0000712394030000019.GIF" wi="251" he="90" />h为步长调节系数,则<maths num="0007" id="cmaths0007"><math><![CDATA[<mrow><msub><mi>F</mi><mi>x</mi></msub><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><msub><mover><mi>S</mi><mo>^</mo></mover><mi>x</mi></msub><mo>=</mo><msub><mi>S</mi><mrow><mi>x</mi><mover><mi>x</mi><mo>^</mo></mover></mrow></msub><mo>=</mo><mo>{</mo><mrow><mo>(</mo><msub><mi>f</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi></msub><mo>+</mo><mi>h</mi><msub><mover><mi>S</mi><mo>^</mo></mover><mrow><mi>x</mi><mo>,</mo><mi>j</mi></mrow></msub><mo>,</mo><msub><mi>u</mi><mi>k</mi></msub><mo>,</mo><msub><mi>w</mi><mi>k</mi></msub><mo>)</mo></mrow><mo>-</mo><msub><mi>f</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi></msub><mo>-</mo><mi>h</mi><msub><mover><mi>S</mi><mo>^</mo></mover><mrow><mi>x</mi><mo>,</mo><mi>j</mi></mrow></msub><mo>,</mo><msub><mi>u</mi><mi>k</mi></msub><mo>,</mo><msub><mi>w</mi><mi>k</mi></msub><mo>)</mo></mrow><mo>)</mo></mrow><mo>/</mo><mn>2</mn><mi>h</mi><mo>,</mo></mrow>]]></math><img file="FDA00007123940300000110.GIF" wi="1368" he="104" /></maths>其中,<img file="FDA0000712394030000021.GIF" wi="95" he="100" />为<img file="FDA0000712394030000022.GIF" wi="65" he="85" />的第j列,根据上式同样可导出下列各式,即<maths num="0008" id="cmaths0008"><math><![CDATA[<mrow><msub><mi>F</mi><mi>w</mi></msub><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><msub><mi>S</mi><mi>w</mi></msub><mo>=</mo><msub><mi>S</mi><mi>xw</mi></msub><mo>=</mo><mo>{</mo><mrow><mo>(</mo><msub><mi>f</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi></msub><mo>,</mo><msub><mi>u</mi><mi>k</mi></msub><mo>,</mo><msub><mi>w</mi><mi>k</mi></msub><mo>+</mo><mi>h</mi><msub><mover><mi>S</mi><mo>^</mo></mover><mrow><mi>x</mi><mo>,</mo><mi>j</mi></mrow></msub><mo>)</mo></mrow><mo>-</mo><msub><mi>f</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi></msub><mo>,</mo><msub><mi>u</mi><mi>k</mi></msub><mo>,</mo><msub><mi>w</mi><mi>k</mi></msub><mo>-</mo><mi>h</mi><msub><mover><mi>S</mi><mo>^</mo></mover><mrow><mi>x</mi><mo>,</mo><mi>j</mi></mrow></msub><mo>)</mo></mrow><mo>)</mo></mrow><mo>/</mo><mn>2</mn><mi>h</mi><mo>}</mo></mrow>]]></math><img file="FDA0000712394030000023.GIF" wi="1360" he="91" /></maths><maths num="0009" id="cmaths0009"><math><![CDATA[<mrow><msub><mi>G</mi><mi>x</mi></msub><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><msub><mover><mi>S</mi><mo>&OverBar;</mo></mover><mi>x</mi></msub><mo>=</mo><msub><mi>S</mi><mrow><mi>y</mi><mover><mi>x</mi><mo>&OverBar;</mo></mover></mrow></msub><mo>=</mo><mo>{</mo><mrow><mo>(</mo><msub><mi>g</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>&OverBar;</mo></mover><mi>k</mi></msub><mo>+</mo><mi>h</mi><msub><mover><mi>S</mi><mo>&OverBar;</mo></mover><mrow><mi>x</mi><mo>,</mo><mi>j</mi></mrow></msub><mo>,</mo><msub><mi>u</mi><mi>k</mi></msub><mo>,</mo><msub><mi>v</mi><mi>k</mi></msub><mo>)</mo></mrow><mo>-</mo><msub><mi>g</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>&OverBar;</mo></mover><mi>k</mi></msub><mo>-</mo><mi>h</mi><msub><mover><mi>S</mi><mo>&OverBar;</mo></mover><mrow><mi>x</mi><mo>,</mo><mi>j</mi></mrow></msub><mo>,</mo><msub><mi>u</mi><mi>k</mi></msub><mo>,</mo><msub><mi>v</mi><mi>k</mi></msub><mo>)</mo></mrow><mo>)</mo></mrow><mo>/</mo><mn>2</mn><mi>h</mi><mo>}</mo></mrow>]]></math><img file="FDA0000712394030000024.GIF" wi="1354" he="97" /></maths><maths num="0010" id="cmaths0010"><math><![CDATA[<mrow><msub><mi>G</mi><mi>v</mi></msub><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><msub><mi>S</mi><mi>v</mi></msub><mo>=</mo><msub><mi>S</mi><mi>yv</mi></msub><mo>=</mo><mo>{</mo><mrow><mo>(</mo><msub><mi>g</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>&OverBar;</mo></mover><mi>k</mi></msub><mo>,</mo><msub><mi>u</mi><mi>k</mi></msub><mo>,</mo><msub><mi>v</mi><mi>k</mi></msub><mo>+</mo><mi>h</mi><msub><mover><mi>S</mi><mo>&OverBar;</mo></mover><mrow><mi>x</mi><mo>,</mo><mi>j</mi></mrow></msub><mo>)</mo></mrow><mo>-</mo><msub><mi>g</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>&OverBar;</mo></mover><mi>k</mi></msub><mo>,</mo><msub><mi>u</mi><mi>k</mi></msub><mo>,</mo><msub><mi>v</mi><mi>k</mi></msub><mo>-</mo><mi>h</mi><msub><mover><mi>S</mi><mo>&OverBar;</mo></mover><mrow><mi>x</mi><mo>,</mo><mi>j</mi></mrow></msub><mo>)</mo></mrow><mo>)</mo></mrow><mo>/</mo><mn>2</mn><mi>h</mi><mo>}</mo><mo>,</mo></mrow>]]></math><img file="FDA0000712394030000025.GIF" wi="1362" he="107" /></maths>G<sub>x</sub>(k)是利用一阶中心差分计算观测方程的偏导数;G<sub>v</sub>(k)是利用一阶中心差分计算观测噪声的偏导数;步骤二、得到基于锂电池系统上一状态预测出当前状态方程<maths num="0011" id="cmaths0011"><math><![CDATA[<mrow><msub><mover><mi>x</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>&ap;</mo><mi>f</mi><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi></msub><mo>,</mo><msub><mi>u</mi><mi>k</mi></msub><mo>,</mo><msub><mi>w</mi><mi>k</mi></msub><mo>)</mo></mrow><mo>,</mo></mrow>]]></math><img file="FDA0000712394030000026.GIF" wi="426" he="100" /></maths>步骤三、得到状态协方差的预测方程<maths num="0012" id="cmaths0012"><math><![CDATA[<mrow><mfenced open='' close=''><mtable><mtr><mtd><mover><mi>P</mi><mo>&OverBar;</mo></mover><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>=</mo><msub><mi>F</mi><mi>x</mi></msub><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mover><mi>P</mi><mo>^</mo></mover><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><msubsup><mi>F</mi><mi>x</mi><mi>T</mi></msubsup><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>+</mo><msub><mi>F</mi><mi>w</mi></msub><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mi>Q</mi><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><msubsup><mi>F</mi><mi>w</mi><mi>T</mi></msubsup><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow></mtd></mtr><mtr><mtd><mo>=</mo><msub><mi>F</mi><mi>x</mi></msub><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><msub><mover><mi>S</mi><mo>^</mo></mover><mi>x</mi></msub><msubsup><mover><mi>S</mi><mo>^</mo></mover><mi>x</mi><mi>T</mi></msubsup><msubsup><mi>F</mi><mi>x</mi><mi>T</mi></msubsup><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>+</mo><msub><mi>F</mi><mi>w</mi></msub><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><msub><mi>S</mi><mi>w</mi></msub><msubsup><mi>S</mi><mi>w</mi><mi>T</mi></msubsup><msubsup><mi>F</mi><mi>w</mi><mi>T</mi></msubsup><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow></mtd></mtr><mtr><mtd><mo>=</mo><msub><mi>S</mi><mrow><mi>x</mi><mover><mi>x</mi><mo>^</mo></mover></mrow></msub><msubsup><mi>S</mi><mrow><mi>x</mi><mover><mi>x</mi><mo>^</mo></mover></mrow><mi>T</mi></msubsup><mo>+</mo><msub><mi>S</mi><mi>xw</mi></msub><msubsup><mi>S</mi><mi>xw</mi><mi>T</mi></msubsup></mtd></mtr></mtable></mfenced><mo>,</mo></mrow>]]></math><img file="FDA0000712394030000027.GIF" wi="910" he="288" /></maths>其中:<img file="FDA0000712394030000028.GIF" wi="115" he="87" />是当前状态的验前状态协方差;F<sub>x</sub>(k)是利用一阶中心差分计算状态方程的偏导数;<img file="FDA0000712394030000029.GIF" wi="116" he="86" />是上一状态的验后状态协方差;F<sub>w</sub>(k)是利用一阶中心差分计算状态噪声的偏导数;Q(k)是状态噪声协方差;步骤四、得到滤波增益方程<maths num="0013" id="cmaths0013"><math><![CDATA[<mrow><mfenced open='' close=''><mtable><mtr><mtd><msub><mi>K</mi><mi>k</mi></msub><mo>=</mo><mover><mi>P</mi><mo>&OverBar;</mo></mover><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><msubsup><mi>G</mi><mi>x</mi><mi>T</mi></msubsup><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><msup><mrow><mo>[</mo><msub><mi>G</mi><mi>x</mi></msub><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mover><mi>P</mi><mo>&OverBar;</mo></mover><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><msubsup><mi>G</mi><mi>x</mi><mi>T</mi></msubsup><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>+</mo><msub><mi>G</mi><mi>v</mi></msub><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mi>R</mi><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><msubsup><mi>G</mi><mi>v</mi><mi>T</mi></msubsup><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>]</mo></mrow><mrow><mo>-</mo><mn>1</mn></mrow></msup></mtd></mtr><mtr><mtd><mo>=</mo><msub><mover><mi>S</mi><mo>&OverBar;</mo></mover><mi>x</mi></msub><msubsup><mover><mi>S</mi><mo>&OverBar;</mo></mover><mi>x</mi><mi>T</mi></msubsup><msup><mrow><mo>(</mo><msub><mi>S</mi><mrow><mi>y</mi><mover><mi>x</mi><mo>&OverBar;</mo></mover></mrow></msub><msubsup><mover><mi>S</mi><mo>&OverBar;</mo></mover><mi>x</mi><mrow><mo>-</mo><mn>1</mn></mrow></msubsup><mo>)</mo></mrow><mi>T</mi></msup><msup><mrow><mo>[</mo><msub><mi>S</mi><mrow><mi>y</mi><mover><mi>x</mi><mo>&OverBar;</mo></mover></mrow></msub><msubsup><mi>S</mi><mrow><mi>y</mi><mover><mi>x</mi><mo>&OverBar;</mo></mover></mrow><mi>T</mi></msubsup><mo>+</mo><msub><mi>S</mi><mi>yv</mi></msub><msubsup><mi>S</mi><mi>yv</mi><mi>T</mi></msubsup><mo>]</mo></mrow><mrow><mo>-</mo><mn>1</mn></mrow></msup></mtd></mtr><mtr><mtd><mo>=</mo><msub><mover><mi>S</mi><mo>&OverBar;</mo></mover><mi>x</mi></msub><msubsup><mi>S</mi><mrow><mi>y</mi><mover><mi>x</mi><mo>&OverBar;</mo></mover></mrow><mi>T</mi></msubsup><msup><mrow><mo>[</mo><msub><mi>S</mi><mrow><mi>y</mi><mover><mi>x</mi><mo>&OverBar;</mo></mover></mrow></msub><msubsup><mi>S</mi><mrow><mi>y</mi><mover><mi>x</mi><mo>&OverBar;</mo></mover></mrow><mi>T</mi></msubsup><mo>+</mo><msub><mi>S</mi><mi>yv</mi></msub><msubsup><mi>S</mi><mi>yv</mi><mi>T</mi></msubsup><mo>]</mo></mrow><mrow><mo>-</mo><mn>1</mn></mrow></msup></mtd></mtr></mtable></mfenced><mo>,</mo></mrow>]]></math><img file="FDA00007123940300000210.GIF" wi="1153" he="302" /></maths>其中:K<sub>k</sub>是卡尔曼滤波增益;G<sub>x</sub>(k)是利用一阶中心差分计算观测方程的偏导数;G<sub>v</sub>(k)是利用一阶中心差分计算观测噪声的偏导数;R(k)是观测噪声协方差;步骤五、根据当前状态的预测值,再收集当前状态的测量值,结合预测值和测量值,我们可以得到当前状态的最优值,得到更新状态的最优值<maths num="0014" id="cmaths0014"><math><![CDATA[<mrow><msub><mover><mi>y</mi><mo>&OverBar;</mo></mover><mi>k</mi></msub><mo>=</mo><mi>g</mi><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>&OverBar;</mo></mover><mi>k</mi></msub><mo>,</mo><msub><mi>u</mi><mi>k</mi></msub><mo>,</mo><msub><mi>v</mi><mi>k</mi></msub><mo>)</mo></mrow></mrow>]]></math><img file="FDA0000712394030000031.GIF" wi="362" he="83" /></maths><maths num="0015" id="cmaths0015"><math><![CDATA[<mrow><msub><mover><mi>x</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>=</mo><msub><mover><mi>x</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>+</mo><msub><mi>K</mi><mi>k</mi></msub><mo>[</mo><msub><mi>y</mi><mi>k</mi></msub><mo>-</mo><msub><mover><mi>y</mi><mo>&OverBar;</mo></mover><mi>k</mi></msub><mo>]</mo><mo>,</mo></mrow>]]></math><img file="FDA0000712394030000032.GIF" wi="529" he="103" /></maths>其中,<img file="FDA0000712394030000033.GIF" wi="66" he="85" />是当前状态的预测输出;<img file="FDA0000712394030000034.GIF" wi="96" he="83" />是当前状态的最优值;y<sub>k</sub>是当前状态实际的测量值;步骤六、得到滤波协方差更新<maths num="0016" id="cmaths0016"><math><![CDATA[<mrow><mfenced open='' close=''><mtable><mtr><mtd><mover><mi>P</mi><mo>^</mo></mover><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>=</mo><mover><mi>P</mi><mo>&OverBar;</mo></mover><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>-</mo><msub><mi>K</mi><mi>k</mi></msub><msub><mi>G</mi><mi>v</mi></msub><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mover><mi>P</mi><mo>&OverBar;</mo></mover><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>=</mo><msub><mover><mi>S</mi><mo>&OverBar;</mo></mover><mi>x</mi></msub><msubsup><mover><mi>S</mi><mo>&OverBar;</mo></mover><mi>x</mi><mi>T</mi></msubsup><mo>-</mo><msub><mi>K</mi><mi>k</mi></msub><msub><mi>G</mi><mi>v</mi></msub><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><msub><mover><mi>S</mi><mo>&OverBar;</mo></mover><mi>x</mi></msub><msubsup><mover><mi>S</mi><mo>&OverBar;</mo></mover><mi>x</mi><mi>T</mi></msubsup></mtd></mtr><mtr><mtd><mo>=</mo><msub><mover><mi>S</mi><mo>&OverBar;</mo></mover><mi>x</mi></msub><msubsup><mover><mi>S</mi><mo>&OverBar;</mo></mover><mi>x</mi><mi>T</mi></msubsup><mo>-</mo><msub><mover><mi>S</mi><mo>&OverBar;</mo></mover><mi>x</mi></msub><msubsup><mi>S</mi><mrow><mi>y</mi><mover><mi>x</mi><mo>&OverBar;</mo></mover></mrow><mi>T</mi></msubsup><msubsup><mi>K</mi><mi>k</mi><mi>T</mi></msubsup><mo>-</mo><msub><mi>K</mi><mi>k</mi></msub><msub><mi>S</mi><mrow><mi>y</mi><mover><mi>x</mi><mo>&OverBar;</mo></mover></mrow></msub><msubsup><mover><mi>S</mi><mo>&OverBar;</mo></mover><mi>x</mi><mi>T</mi></msubsup><mo>+</mo><msub><mover><mi>S</mi><mo>&OverBar;</mo></mover><mi>x</mi></msub><msubsup><mi>S</mi><mrow><mi>y</mi><mover><mi>x</mi><mo>&OverBar;</mo></mover></mrow><mi>T</mi></msubsup><msubsup><mi>K</mi><mi>k</mi><mi>T</mi></msubsup></mtd></mtr><mtr><mtd><mo>=</mo><msub><mover><mi>S</mi><mo>&OverBar;</mo></mover><mi>x</mi></msub><msubsup><mover><mi>S</mi><mo>&OverBar;</mo></mover><mi>x</mi><mi>T</mi></msubsup><mo>-</mo><msub><mover><mi>S</mi><mo>&OverBar;</mo></mover><mi>x</mi></msub><msubsup><mi>S</mi><mrow><mi>y</mi><mover><mi>x</mi><mo>&OverBar;</mo></mover></mrow><mi>T</mi></msubsup><msubsup><mi>K</mi><mi>k</mi><mi>T</mi></msubsup><mo>-</mo><msub><mi>K</mi><mi>k</mi></msub><msub><mi>S</mi><mrow><mi>y</mi><mover><mi>x</mi><mo>&OverBar;</mo></mover></mrow></msub><msubsup><mover><mi>S</mi><mo>&OverBar;</mo></mover><mi>x</mi><mi>T</mi></msubsup><mo>+</mo><msub><mi>K</mi><mi>k</mi></msub><msub><mi>S</mi><mrow><mi>y</mi><mover><mi>x</mi><mo>&OverBar;</mo></mover></mrow></msub><msubsup><mrow><msubsup><mi>S</mi><mrow><mi>y</mi><mover><mi>x</mi><mo>&OverBar;</mo></mover></mrow><mi>T</mi></msubsup><mi>K</mi></mrow><mi>k</mi><mi>T</mi></msubsup><mo>+</mo><msub><mi>K</mi><mi>k</mi></msub><msub><mi>S</mi><mi>yv</mi></msub><msubsup><mi>S</mi><mi>yv</mi><mi>T</mi></msubsup><msubsup><mi>K</mi><mi>k</mi><mi>T</mi></msubsup></mtd></mtr><mtr><mtd><mo>=</mo><mo>[</mo><msub><mover><mi>S</mi><mo>&OverBar;</mo></mover><mi>x</mi></msub><mo>-</mo><msub><mi>K</mi><mi>k</mi></msub><msub><mi>S</mi><mrow><mi>y</mi><mover><mi>x</mi><mo>&OverBar;</mo></mover></mrow></msub><msub><mi>K</mi><mi>k</mi></msub><msub><mi>S</mi><mi>yv</mi></msub><mo>]</mo><msup><mrow><mo>[</mo><msub><mover><mi>S</mi><mo>&OverBar;</mo></mover><mi>x</mi></msub><mo>-</mo><msub><mi>K</mi><mi>k</mi></msub><msub><mi>S</mi><mrow><mi>y</mi><mover><mi>x</mi><mo>&OverBar;</mo></mover></mrow></msub><msub><mi>K</mi><mi>k</mi></msub><msub><mi>S</mi><mi>yv</mi></msub><mo>]</mo></mrow><mi>T</mi></msup></mtd></mtr></mtable></mfenced><mo>,</mo></mrow>]]></math><img file="FDA0000712394030000035.GIF" wi="1268" he="418" /></maths>其中,<img file="FDA0000712394030000036.GIF" wi="122" he="90" />是当前状态的验后状态协方差。
地址 300072 天津市南开区卫津路92号