python实现EKF的CTRV模型

python实现CTRV模型的扩展卡尔曼滤波的代码，里面有详细解释和如何运行代码。具体的参考我的博客：https://blog.csdn.net/O_MMMM_O/article/details/106078679

import plotly.offline as py
from plotly.graph_objs import *
import pandas as pd
import math

py.init_notebook_mode（）

my_cols = [‘px_est‘ ‘py_est‘ ‘vx_est‘ ‘vy_est‘ ‘px_meas‘ ‘py_meas‘ ‘px_gt‘ ‘py_gt‘ ‘vx_gt‘ ‘vy_gt‘]
with open（‘output.txt‘） as f:
    table_ekf_output = pd.read_table（f sep=‘ ‘ header=None names=my_cols lineterminator=‘\n‘）

    # table_ekf_output

import plotly.offline as py
from plotly.graph_objs import *

# Measurements
trace2 = Scatter（
    x=table_ekf_output[‘px_meas‘]
    y=table_ekf_output[‘py_meas‘]
    xaxis=‘x2‘
    yaxis=‘y2‘
    name=‘Measurements‘
    mode = ‘markers‘
）

# estimations
trace1 = Scatter（
    x=table_ekf_output[‘px_est‘]
    y=table_ekf_output[‘py_est‘]
    xaxis=‘x2‘
    yaxis=‘y2‘
    name=‘KF- Estimate‘
    mode=‘markers‘
）

# Ground Truth
trace3 = Scatter（
    x=table_ekf_output[‘px_gt‘]
    y=table_ekf_output[‘py_gt‘]
    xaxis=‘x2‘
    yaxis=‘y2‘
    name=‘Ground Truth‘
    mode=‘markers‘
）

data = [trace1 trace2 trace3]

layout = Layout（
    xaxis2=dict（

        anchor=‘x2‘
        title=‘px‘
    ）
    yaxis2=dict（

        anchor=‘y2‘
        title=‘py‘
    ）
）

fig = Figure（data=data layout=layout）
py.plot（fig filename=‘EKF‘）

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

    .......     66250  2019-12-02 20:03  EKF_CTRV\data_synthetic.txt

    .......   3033924  2019-12-02 20:03  EKF_CTRV\EKF.html

    .......     48587  2019-12-02 20:03  EKF_CTRV\output.txt

    .......     66250  2019-12-02 20:03  EKF_CTRV\obj_pose-laser-radar-synthetic-input.txt

    .......     48587  2019-12-02 20:03  EKF_CTRV\myEKF\output.txt

    .......     12547  2019-12-02 20:03  EKF_CTRV\myEKF\EKF_CTVR.ipynb

    .......      7687  2019-12-02 20:03  EKF_CTRV\myEKF\EKF_CTVR.py

    .......      1232  2019-12-02 20:03  EKF_CTRV\myEKF\plot1.py

    .......     44234  2019-12-02 20:03  EKF_CTRV\myEKF\newplot.png

    .......   3033924  2019-12-02 20:03  EKF_CTRV\myEKF\EKF.html

    .......     66250  2019-12-02 20:03  EKF_CTRV\myEKF\data_synthetic.txt

    .......       137  2019-12-02 20:03  EKF_CTRV\myEKF\README.md

    .......     12547  2019-12-02 20:03  EKF_CTRV\EKF_CTVR.ipynb

    .......     44234  2019-12-02 20:03  EKF_CTRV\newplot.png

    .......     12556  2019-12-02 20:03  EKF_CTRV\.ipynb_checkpoints\EKF_CTVR-checkpoint.ipynb

    .......     42135  2019-12-02 20:03  EKF_CTRV\data\output.txt

    .......   3028273  2019-12-02 20:03  EKF_CTRV\data\EKF.html

    .......     66250  2019-12-02 20:03  EKF_CTRV\data\data_synthetic.txt

    .......      1232  2019-12-02 20:03  EKF_CTRV\data\plot1.py

    .......       137  2019-12-02 20:03  EKF_CTRV\README.md

    .......     48587  2019-12-02 20:03  EKF_CTRV\output.csv

    .......      1232  2019-12-02 20:03  EKF_CTRV\plot1.py

     目录          0  2019-12-02 20:03  EKF_CTRV\myEKF

    ...D.H.         0  2019-12-02 20:03  EKF_CTRV\.ipynb_checkpoints

     目录          0  2019-12-02 20:03  EKF_CTRV\data

     目录          0  2019-12-02 20:03  EKF_CTRV

----------- ---------  ---------- -----  ----

              9686792                    26