迭代平方根中心差分卡尔曼粒子滤波方法

摘要：

本发明公开了一种迭代平方根中心差分卡尔曼粒子滤波方法，其目的是解决现有的卡尔曼滤波方法跟踪精度低的技术问题。技术方案是根据非线性、非高斯系统目标跟踪的特点，引入Levenberg-Marquardt优化方法，修正状态协方差；利用迭代的平方根CDKF和所引入Levenberg-Marquardt优化方法共同来产生建议分布，建立迭代平方根中心差分卡尔曼粒子滤波方法。由于利用迭代的平方根CDKF和引入Levenberg-Marquardt优化方法共同来产生建议分布，粒子滤波(PF)、迭代扩展卡尔曼粒子滤波(IEKFPF)、Sigma粒子滤波SPPF、迭代平方根中心差分卡尔曼粒子滤波方法(ISPPF)粒子数均为100，迭代平方根中心差分卡尔曼粒子滤波方法可以使跟踪精度提高至少30多倍，均值为0.0147，方差为0.00000394，还具有很好的稳定性。

专利类型：发明专利

申请/专利号：CN201010142277.X

申请日期：2010-04-08

公开/公告号：CN101820269A

公开/公告日：1900-01-01

主分类号：H03H21/00(2006.01)I

申请/专利权人:西北工业大学

发明/设计人:李国辉;李亚安;何健;杨宏;崔琳

主申请人地址:710072 陕西省西安市友谊西路127号

专利代理机构:西北工业大学专利中心 61204

代理人:黄毅新

国别省市代码:陕西;61

权利要求：

一种迭代平方根中心差分卡尔曼粒子滤波方法，其特征在于包括下述步骤：(a)根据非线性、非高斯系统目标跟踪的特点，采用Levenberg-Marquardt优化方法，修正状态协方差；系统方程： <mrow> <msub> <mi>x</mi> <mi>k</mi> </msub> <mo>=</mo> <mi>f</mi> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>,</mo> <msub> <mi>w</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>&LeftRightArrow;</mo> <mi>p</mi> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>k</mi> </msub> <mo>|</mo> <msub> <mi>x</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow>量测方程： <mrow> <msub> <mi>y</mi> <mi>k</mi> </msub> <mo>=</mo> <mi>h</mi> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>k</mi> </msub> <mo>,</mo> <msub> <mi>v</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>&LeftRightArrow;</mo> <mi>p</mi> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mi>k</mi> </msub> <mo>|</mo> <msub> <mi>x</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow>式中，f和h为系统状态和量测的非线性变换，xk，yk分别是系统k时刻的状态值和量测值，wk，vk分别是系统k时刻的状态噪声和测量噪声；对基于线性化方法的卡尔曼滤波，定义代价函数C(xk)为 <mrow> <mi>C</mi> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>k</mi> </msub> <mo>-</mo> <msub> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <msup> <mover> <mi>P</mi> <mo>&OverBar;</mo> </mover> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>k</mi> </msub> <mo>-</mo> <msub> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>+</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mi>k</mi> </msub> <mo>-</mo> <mi>h</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>)</mo> </mrow> <mi>T</mi> </msup> <msup> <mover> <mi>R</mi> <mo>&OverBar;</mo> </mover> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mi>k</mi> </msub> <mo>-</mo> <mi>h</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow>(b)利用迭代的平方根CDKF和步骤(a)共同来产生建议分布，建立迭代平方根中心差分卡尔曼粒子滤波方法；1)初始值： <mrow> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> <mo>=</mo> <mi>E</mi> <mo>[</mo> <msub> <mi>x</mi> <mn>0</mn> </msub> <mo>]</mo> <mo>,</mo> <msub> <mi>S</mi> <mn>0</mn> </msub> <mo>=</mo> <mi>chol</mi> <mo>{</mo> <mi>E</mi> <mo>[</mo> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mn>0</mn> </msub> <mo>-</mo> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> <mo>)</mo> </mrow> <msup> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mn>0</mn> </msub> <mo>-</mo> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>]</mo> <mo>}</mo> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> </mrow>2)构造2L+1维Sigma点： <mrow> <msub> <mi>&chi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mo>[</mo> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <mi>&eta;</mi> <msub> <mi>S</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <mi>&eta;</mi> <msub> <mi>S</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>]</mo> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>5</mn> <mo>)</mo> </mrow> </mrow>3)时间更新：χk|k-1＝f(χk-1)????????????????(6) <mrow> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mi>k</mi> <mo>-</mo> </msubsup> <mo>=</mo> <munderover> <mi>&Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <mi>L</mi> </mrow> </munderover> <msubsup> <mi>W</mi> <mi>i</mi> <mi>m</mi> </msubsup> <msub> <mi>&chi;</mi> <mrow> <mi>i</mi> <mo>.</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>7</mn> <mo>)</mo> </mrow> </mrow> <mrow> <msubsup> <mi>S</mi> <mi>k</mi> <mo>-</mo> </msubsup> <mo>=</mo> <mi>qr</mi> <mo>{</mo> <mo>[</mo> <msqrt> <msubsup> <mi>W</mi> <mn>1</mn> <mi>c</mi> </msubsup> </msqrt> <mrow> <mo>(</mo> <msub> <mi>&chi;</mi> <mrow> <mn>1</mn> <mo>:</mo> <mn>2</mn> <mi>L</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mi>k</mi> <mo>-</mo> </msubsup> <mo>)</mo> </mrow> <msup> <mrow> <mo>(</mo> <msub> <mi>&chi;</mi> <mrow> <mn>1</mn> <mo>:</mo> <mn>2</mn> <mi>L</mi> <mo>.</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mi>k</mi> <mo>-</mo> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <msqrt> <msup> <mi>R</mi> <mi>w</mi> </msup> </msqrt> <mo>]</mo> <mo>}</mo> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>8</mn> <mo>)</mo> </mrow> </mrow> <mrow> <msubsup> <mi>S</mi> <mi>k</mi> <mo>-</mo> </msubsup> <mo>=</mo> <mi>cholupdate</mi> <mo>{</mo> <msubsup> <mi>S</mi> <mi>k</mi> <mo>-</mo> </msubsup> <mo>,</mo> <msub> <mi>&chi;</mi> <mrow> <mn>0</mn> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>-</mo> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mi>k</mi> <mo>-</mo> </msubsup> <mo>,</mo> <msubsup> <mi>W</mi> <mn>0</mn> <mi>c</mi> </msubsup> <mo>}</mo> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> </mrow>4)测量更新： <mrow> <msub> <mi>X</mi> <mn>1</mn> </msub> <mo>=</mo> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mi>k</mi> <mo>-</mo> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>10</mn> <mo>)</mo> </mrow> </mrow>for?i＝1:m <mrow> <msub> <mi>&chi;</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mo>[</mo> <msub> <mi>X</mi> <mi>i</mi> </msub> <msub> <mi>X</mi> <mi>i</mi> </msub> <mo>+</mo> <mi>h</mi> <msubsup> <mi>S</mi> <mi>k</mi> <mo>-</mo> </msubsup> <msub> <mi>X</mi> <mi>i</mi> </msub> <mo>-</mo> <mi>h</mi> <msubsup> <mi>S</mi> <mi>k</mi> <mo>-</mo> </msubsup> <mo>]</mo> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>12</mn> <mo>)</mo> </mrow> </mrow>uk|k-1＝g(χk|k-1)???????????????(13) <mrow> <msubsup> <mover> <mi>y</mi> <mo>^</mo> </mover> <mi>k</mi> <mo>-</mo> </msubsup> <mo>=</mo> <munderover> <mi>&Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <mi>L</mi> </mrow> </munderover> <msubsup> <mi>W</mi> <mi>i</mi> <mi>m</mi> </msubsup> <msub> <mi>y</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>14</mn> <mo>)</mo> </mrow> </mrow> <mrow> <msub> <mi>S</mi> <msub> <mi>y</mi> <mi>k</mi> </msub> </msub> <mo>=</mo> <mi>qr</mi> <mo>{</mo> <mo>[</mo> <msqrt> <msubsup> <mi>W</mi> <mi>i</mi> <mi>c</mi> </msubsup> </msqrt> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mrow> <mn>1</mn> <mo>:</mo> <mn>2</mn> <mi>L</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>-</mo> <msub> <mover> <mi>y</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> <mo>)</mo> </mrow> <msqrt> <msubsup> <mi>R</mi> <mi>k</mi> <mi>v</mi> </msubsup> </msqrt> <mo>]</mo> <mo>}</mo> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>15</mn> <mo>)</mo> </mrow> </mrow> <mrow> <msub> <mi>S</mi> <msub> <mi>y</mi> <mi>k</mi> </msub> </msub> <mo>=</mo> <mi>cholupdate</mi> <mo>{</mo> <msub> <mi>s</mi> <msub> <mi>y</mi> <mi>k</mi> </msub> </msub> <mo>,</mo> <msub> <mi>y</mi> <mrow> <mi>o</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>-</mo> <msub> <mover> <mi>y</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> <mo>,</mo> <msubsup> <mi>W</mi> <mn>0</mn> <mi>c</mi> </msubsup> <mo>}</mo> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>16</mn> <mo>)</mo> </mrow> </mrow> <mrow> <msub> <mi>P</mi> <mrow> <msub> <mi>X</mi> <mi>k</mi> </msub> <msub> <mi>y</mi> <mi>k</mi> </msub> </mrow> </msub> <mo>=</mo> <munderover> <mi>&Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <mi>L</mi> </mrow> </munderover> <msubsup> <mi>W</mi> <mi>i</mi> <mi>c</mi> </msubsup> <mrow> <mo>(</mo> <msub> <mi>&chi;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mi>k</mi> <mo>-</mo> </msubsup> <mo>)</mo> </mrow> <msup> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msubsup> <mover> <mi>y</mi> <mo>^</mo> </mover> <mi>k</mi> <mo>-</mo> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>17</mn> <mo>)</mo> </mrow> </mrow> <mrow> <msub> <mi>K</mi> <mi>k</mi> </msub> <mo>=</mo> <mrow> <mo>(</mo> <msub> <mi>P</mi> <mrow> <msub> <mi>X</mi> <mi>k</mi> </msub> <msub> <mi>y</mi> <mi>k</mi> </msub> </mrow> </msub> <mo>/</mo> <msubsup> <mi>S</mi> <msub> <mi>y</mi> <mi>k</mi> </msub> <mi>T</mi> </msubsup> <mo>)</mo> </mrow> <msub> <mi>S</mi> <msub> <mi>y</mi> <mi>k</mi> </msub> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>18</mn> <mo>)</mo> </mrow> </mrow> <mrow> <msub> <mi>X</mi> <mrow> <mi>i</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>X</mi> <mi>i</mi> </msub> <mo>+</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mi>k</mi> </msub> <mo>-</mo> <msubsup> <mover> <mi>y</mi> <mo>^</mo> </mover> <mi>k</mi> <mo>-</mo> </msubsup> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>19</mn> <mo>)</mo> </mrow> </mrow>end <mrow> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>X</mi> <mrow> <mi>i</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>20</mn> <mo>)</mo> </mrow> </mrow> <mrow> <mi>U</mi> <mo>=</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <msub> <mi>S</mi> <msub> <mi>y</mi> <mi>k</mi> </msub> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>21</mn> <mo>)</mo> </mrow> </mrow> <mrow> <msub> <mi>S</mi> <mi>k</mi> </msub> <mo>=</mo> <mi>cholupdate</mi> <mo>{</mo> <msubsup> <mi>S</mi> <mi>k</mi> <mo>-</mo> </msubsup> <mo>,</mo> <mi>U</mi> <mo>,</mo> <mo>-</mo> <mn>1</mn> <mo>}</mo> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>22</mn> <mo>)</mo> </mrow> </mrow>式中，Rw为状态过程噪声协方差矩阵，Rv为测量噪声协方差矩阵；h≥1为中心差分区间长度，w1＝1/(4h2)和w2＝(h2-1)/(4h4)，L是状态向量的维数，Levenberg-Marquardt参数μi的选为0.1；迭代的平方根CDKF粒子滤波方法(ISPPF)，1)当k＝0时，采样粒子i＝1，...，N。2)重要性采样：将初始粒子代入迭代的平方根CDKF中产生重要性函数分布，并采样粒子 <mrow> <msubsup> <mi>x</mi> <mi>k</mi> <mi>i</mi> </msubsup> <mo>~</mo> <mi>N</mi> <mrow> <mo>(</mo> <msubsup> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> <mi>i</mi> </msubsup> <mo>,</mo> <msubsup> <mi>S</mi> <mi>k</mi> <mi>i</mi> </msubsup> <mo>)</mo> </mrow> <mo>;</mo> </mrow>3)重要性权值：当i＝2，...，N时， <mrow> <msubsup> <mi>w</mi> <mi>k</mi> <mi>i</mi> </msubsup> <mo>=</mo> <mfrac> <mrow> <mi>p</mi> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mi>k</mi> </msub> <mo>|</mo> <msubsup> <mi>x</mi> <mi>k</mi> <mi>i</mi> </msubsup> <mo>)</mo> </mrow> <mi>p</mi> <mrow> <mo>(</mo> <msubsup> <mi>x</mi> <mi>k</mi> <mi>i</mi> </msubsup> <mo>|</mo> <msubsup> <mi>x</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <mo>)</mo> </mrow> </mrow> <mrow> <mi>N</mi> <mrow> <mo>(</mo> <msubsup> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> <mi>i</mi> </msubsup> <mo>,</mo> <msubsup> <mi>P</mi> <mi>k</mi> <mi>i</mi> </msubsup> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>23</mn> <mo>)</mo> </mrow> </mrow>归一化权值： <mrow> <msubsup> <mover> <mi>w</mi> <mo>~</mo> </mover> <mi>k</mi> <mi>i</mi> </msubsup> <mo>=</mo> <mfrac> <msubsup> <mi>w</mi> <mi>k</mi> <mi>i</mi> </msubsup> <mrow> <munderover> <mi>&Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </munderover> <msubsup> <mi>w</mi> <mi>k</mi> <mi>i</mi> </msubsup> </mrow> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>24</mn> <mo>)</mo> </mrow> </mrow>4)重采样：从xki中根据重要性权值重新采样得到新的N个粒子并重新分配权值FSA000000751757000110.tif,FSA00000075175700029.tif,FSA000000751757000213.tif,FSA000000751757000214.tif