主权项 |
基于分层空间的RB化粒子滤波算法,其特征在于包括如下步骤:步骤一、粒子初始化根据目标运动模型先验概率p(m(O))选取N个粒子,记为m<sup>(i)</sup>(0)对应的权值<img file="FDA0000927255310000011.GIF" wi="455" he="157" />根据目标的状态先验概率获得<img file="FDA0000927255310000012.GIF" wi="679" he="116" />则初始状态集合可以定义为<maths num="0001"><math><![CDATA[<mrow><msubsup><mrow><mo>{</mo><mrow><msup><mi>m</mi><mrow><mo>(</mo><mi>i</mi><mo>)</mo></mrow></msup><mrow><mo>(</mo><mn>0</mn><mo>)</mo></mrow><mo>,</mo><mrow><mo>(</mo><mrow><msup><mover><mi>X</mi><mo>^</mo></mover><mrow><mo>(</mo><mi>i</mi><mo>)</mo></mrow></msup><mo>,</mo><msup><mi>P</mi><mrow><mo>(</mo><mi>i</mi><mo>)</mo></mrow></msup><mrow><mo>(</mo><mn>0</mn><mo>)</mo></mrow></mrow><mo>)</mo></mrow><mo>,</mo><msup><mover><mi>w</mi><mo>~</mo></mover><mrow><mo>(</mo><mi>i</mi><mo>)</mo></mrow></msup></mrow><mo>}</mo></mrow><mrow><mi>i</mi><mo>=</mo><mn>1</mn></mrow><mi>N</mi></msubsup><mo>;</mo></mrow>]]></math><img file="FDA0000927255310000013.GIF" wi="1284" he="139" /></maths>步骤二、重要性采样选取最优分布p(m(k)|m(k‑1),Z<sub>1:k</sub>)作为重要性函数:<maths num="0002"><math><![CDATA[<mfenced open = "" close = ""><mtable><mtr><mtd><mrow><mi>p</mi><mrow><mo>(</mo><mrow><mi>m</mi><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>|</mo><mi>m</mi><mrow><mo>(</mo><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow><mo>)</mo></mrow><mo>=</mo><msup><mi>m</mi><mrow><mo>(</mo><mi>i</mi><mo>)</mo></mrow></msup><mrow><mo>(</mo><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow><mo>)</mo></mrow><mo>,</mo><msub><mi>Z</mi><mrow><mn>1</mn><mo>:</mo><mi>k</mi></mrow></msub></mrow><mo>)</mo></mrow></mrow></mtd></mtr><mtr><mtd><mrow><mo>=</mo><mfrac><mrow><mi>p</mi><mrow><mo>(</mo><mrow><mi>Z</mi><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>|</mo><mi>m</mi><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>,</mo><msub><mi>Z</mi><mrow><mn>1</mn><mo>:</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub></mrow><mo>)</mo></mrow><mi>p</mi><mrow><mo>(</mo><mrow><mi>m</mi><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>|</mo><mi>m</mi><mrow><mo>(</mo><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow><mo>)</mo></mrow><mo>=</mo><msup><mi>m</mi><mrow><mo>(</mo><mi>i</mi><mo>)</mo></mrow></msup><mrow><mo>(</mo><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow><mo>)</mo></mrow></mrow><mo>)</mo></mrow></mrow><mrow><mi>p</mi><mrow><mo>(</mo><mrow><mi>Z</mi><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>|</mo><msub><mi>Z</mi><mrow><mn>1</mn><mo>:</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>,</mo><mi>m</mi><mrow><mo>(</mo><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow><mo>)</mo></mrow><mo>=</mo><msup><mi>m</mi><mrow><mo>(</mo><mi>i</mi><mo>)</mo></mrow></msup><mrow><mo>(</mo><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow><mo>)</mo></mrow></mrow><mo>)</mo></mrow></mrow></mfrac></mrow></mtd></mtr></mtable></mfenced>]]></math><img file="FDA0000927255310000014.GIF" wi="1224" he="346" /></maths>根据概率,可以得到根据重要性采样得到的新的粒子<maths num="0003"><math><![CDATA[<mrow><msubsup><mrow><mo>{</mo><mrow><msup><mi>m</mi><mrow><mo>(</mo><mi>i</mi><mo>)</mo></mrow></msup><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow></mrow><mo>}</mo></mrow><mrow><mi>i</mi><mo>=</mo><mn>1</mn></mrow><mi>N</mi></msubsup><mo>;</mo></mrow>]]></math><img file="FDA0000927255310000015.GIF" wi="525" he="122" /></maths>步骤三、粒子权值计算根据式更新粒子的权值得到<img file="FDA0000927255310000016.GIF" wi="527" he="120" />w(k)∝w(k‑1)p(Z(k)|m(k‑1),Z<sub>1:k‑1</sub>);步骤四、状态参数更新根据每个粒子运动模型,采用交互式多模型得到<maths num="0004"><math><![CDATA[<mrow><mo>{</mo><msup><mover><mi>X</mi><mo>^</mo></mover><mrow><mo>(</mo><mi>i</mi><mo>)</mo></mrow></msup><mrow><mo>(</mo><mi>k</mi><mo>-</mo><mn>1</mn><mo>)</mo></mrow><mo>,</mo><msup><mi>P</mi><mrow><mo>(</mo><mi>i</mi><mo>)</mo></mrow></msup><mrow><mo>(</mo><mi>k</mi><mo>-</mo><mn>1</mn><mo>)</mo></mrow><mo>}</mo><mo>→</mo><mo>{</mo><msup><mover><mi>X</mi><mo>^</mo></mover><mrow><mo>(</mo><mi>j</mi><mo>)</mo></mrow></msup><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>,</mo><msup><mi>P</mi><mrow><mo>(</mo><mi>i</mi><mo>)</mo></mrow></msup><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>}</mo><mo>;</mo></mrow>]]></math><img file="FDA0000927255310000017.GIF" wi="1813" he="129" /></maths>步骤五、结果输出根据式<maths num="0005"><math><![CDATA[<mrow><mover><mi>x</mi><mo>^</mo></mover><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>=</mo><munderover><mo>Σ</mo><mrow><mi>i</mi><mo>=</mo><mn>1</mn></mrow><mi>N</mi></munderover><msup><mover><mi>w</mi><mo>~</mo></mover><mrow><mo>(</mo><mi>i</mi><mo>)</mo></mrow></msup><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><msup><mover><mi>X</mi><mo>^</mo></mover><mrow><mo>(</mo><mi>i</mi><mo>)</mo></mrow></msup><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow></mrow>]]></math><img file="FDA0000927255310000018.GIF" wi="753" he="170" /></maths>和式<maths num="0006"><math><![CDATA[<mrow><mi>P</mi><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>=</mo><munderover><mi>Σ</mi><mrow><mi>i</mi><mo>=</mo><mn>1</mn></mrow><mi>N</mi></munderover><msup><mover><mi>w</mi><mo>~</mo></mover><mrow><mo>(</mo><mi>i</mi><mo>)</mo></mrow></msup><mrow><mo>(</mo><msup><mi>P</mi><mrow><mo>(</mo><mi>i</mi><mo>)</mo></mrow></msup><mo>(</mo><mi>k</mi><mo>)</mo><mo>+</mo><mo>(</mo><mrow><msup><mover><mi>X</mi><mo>^</mo></mover><mrow><mo>(</mo><mi>i</mi><mo>)</mo></mrow></msup><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>-</mo><mover><mi>X</mi><mo>^</mo></mover><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow></mrow><mo>)</mo><msup><mrow><mo>(</mo><mrow><msup><mover><mi>X</mi><mo>^</mo></mover><mrow><mo>(</mo><mi>i</mi><mo>)</mo></mrow></msup><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>-</mo><mover><mi>X</mi><mo>^</mo></mover><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow></mrow><mo>)</mo></mrow><mi>T</mi></msup><mo>)</mo></mrow><mo>,</mo></mrow>]]></math><img file="FDA0000927255310000019.GIF" wi="1520" he="174" /></maths>输出状态的均值和方差。 |