主要内容

使用时变卡尔曼滤波器的状态估计

这个示例使用:

打开脚本

这个例子展示了如何使用时变线性系统的状态估计模型的卡尔曼滤波器。金宝app您使用的卡尔曼滤波块系统辨识工具箱/估计库地面车辆的位置和速度估计基于噪声测量,如GPS传感器测量位置。卡尔曼滤波器的植物模型时变噪声特性。

介绍

你想估计地面车辆的位置和速度在北部和东部的方向。车辆可以在二维空间自由移动,没有任何约束。你设计一个多功能导航和跟踪系统,可用于任何对象,而不仅仅是一辆车。

美元美元x_e (t)和x_n (t)美元是汽车的东部和北部从原点位置,\θ(t)美元汽车从东方向和吗u_ \ psi (t)美元车辆的转向角。元新台币是连续时间变量。

仿真软件金宝app模型包含两个主要部分:车辆模型和卡尔曼滤波。这些都是在以下部分作进一步的解释。

open_system (“ctrlKalmanNavigationExample”);

汽车模型

履带式汽车是用一个简单的质点模型表示:

$ $ \压裂{d} {dt} \离开[& # xA; \开始{数组}{c} & # xA;x_e (t) \ \ & # xA;x_n (t) \ \ & # xA;s (t) \ \ & # xA;\θ(t) & # xA; \{数组}\]= \左右结束[& # xA; \开始{数组}{c} & # xA;s (t) \ cosθ(t)) (\ \ \ & # xA;s (t) \ sinθ(t)) (\ \ \ & # xA;(P \;\压裂{u_T (t)} {s (t)} - \;重金属镉\; s(t)^2) / m \\ s(t) \tan(u_\psi(t)) / L \end{array} \right] $$

车辆的状态是:

数组$ $ \开始{}{你}& # xA; x_e (t) \;& # 38;\ textnormal{东位置}\;[m] \ \ & # xA; x_n (t) \;& # 38;\ textnormal{北位置}\;[m] \ \ & # xA; s (t) \;& # 38;\ textnormal{速度}\;(米/秒)\ \ & # xA; \θ(t) \; & \textnormal{Orientation from east} \; [deg] \\ \end{array} $$

车辆参数:

数组$ $ \开始{}{你}& # xA; P = 100000 \;& # 38;\ textnormal{发动机功率峰值}\;[W] \ \ & # xA; A = 1 \;& # 38;\ textnormal{锋面}\;(m ^ 2) \ \ & # xA;重金属镉= 0.3 \;& # 38;\ textnormal{阻力系数}\;(无单位)\ \ & # xA; m = 1250 \; & \textnormal{Vehicle mass} \; [kg] \\ L=2.5 \; & \textnormal{Wheelbase length} \; [m] \\ \end{array} $$

和控制输入:

数组$ $ \开始{}{你}& # xA; u_T (t) \;& # 38;\ textnormal{油门位置的范围1和1}\;(无单位)\ \ & # xA; u_ \ psi (t) \;& # 38;\ textnormal{转向角}\;[度]\ \ & # xA; \{数组}$ $

的纵向动力学模型忽略轮胎滚动阻力。模型的横向动力学假设所需的转向角可以瞬间实现,忽略惯性的偏航力矩。

汽车模型的实现ctrlKalmanNavigationExample /车辆模型子系统。仿真软件金宝app模型包含两个PI控制器跟踪期望的方向和速度的汽车ctrlKalmanNavigationExample /速度和定位跟踪子系统。这允许您指定汽车各种操作条件和测试卡尔曼滤波器的性能。

卡尔曼滤波器设计

卡尔曼滤波是一种算法来估计未知变量的兴趣基于线性模型。这个线性模型描述估计变量随时间的演变响应模型初始条件以及已知和未知模型输入。在本例中,您估计以下参数/变量:

$ $ {x} [n] = \ \帽子离开[& # xA;c \开始{数组}{}& # xA;{x} \帽子_e [n] \ \ & # xA;{x} \帽子_n”[n] \ \ & # xA;{\ \帽子点{x}} _e [n] \ \ & # xA;{\ \帽子点{x}} _n”[n] & # xA;结束\{数组}& # xA;\右]& # xA; $ $

在哪里

数组$ $ \开始{}{你}& # xA; \帽子{x} _e [n] \;& # 38;\ textnormal{东位置估计}\;[m] \ \ & # xA; \帽子{x} _n”[n] \;& # 38;\ textnormal{北位置估计}\;[m] \ \ & # xA; \帽子{\点{x}} _e [n] \;& # 38;\ textnormal{东速度估计}\;(米/秒)\ \ & # xA; \帽子{\点{x}} _n”[n] \; & \textnormal{North velocity estimate} \; [m/s] \\ \end{array} $$

的$ \点{x} $表示速度和导数算子。n美元是离散指数。模型中使用卡尔曼滤波器的形式:

数组$ $ \开始{}{rl} & # xA; \帽子{x} (n + 1) & # 38; = \帽子{x} [n] + Gw [n] \ \ & # xA; y [n] & # 38; = C \帽子{x} [n] + v [n] & # xA; \{数组}$ $

在哪里$ \帽子{x} $状态向量,y美元的测量,w美元过程噪声和五美元是测量噪声。卡尔曼滤波器假设w美元和五美元是零均值,独立随机变量方差已知的吗美元$ E [ww ^ T] =问,E (vv ^ T) = R美元,美元$ E [wv ^ T] = N。这里,A、G和C矩阵:

左$ $ = \ [& # xA;\开始{数组}{c c c c} & # xA;1 & # 38岁;0 & # 38;T_s & # 38;0 xA \ \ & #;0 & # 38;1 & # 38岁;0 & # 38;T_s \ \ & # xA; 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array} \right] $$

$ $ G =左\ [& # xA;开始\{数组}{c c} & # xA;T_s / 2 & # 38;0 xA \ \ & #;0 & # 38;T_s / 2 \ \ & # xA;1 & # 38岁;0 xA \ \ & #;0 & # 38;1 & # xA; \end{array} \right] $$

左$ $ C = \ [& # xA;\开始{数组}{c c c c} & # xA;1 & # 38岁;0 & # 38;0 & # 38;0 xA \ \ & #;0 & # 38;1 & # 38岁;0 & # 38;0 xA & #; \end{array} \right] $$

在哪里美元$ T_s = 1 \; [s]

第三行和G模型的东速度随机漫步:$ \帽子{\点{x}} _e (n + 1) ={\点{x}} \帽子_e [n] + w_1美元[n]。在现实中,位置是一个连续时间变量和速度随时间的积分$ \压裂{d} {dt} {x} _e = \ \帽子帽子{\点{x}} _e美元。的第一行和G代表一个离散近似运动关系:美元(\帽子{x} _e (n + 1) - \帽子{x} _e [n]) / Ts =(\帽子{\点{x}} _e (n + 1) + \帽子{\点{x}} _e [n]) / 2美元。第二行和第四行和G表示相同的北速度和位置之间的关系。

C矩阵表示,只有位置测量。位置传感器,如GPS、提供这些测量的采样率1 hz。测量噪声的方差五美元,被指定为R矩阵R = 50美元。自R被指定为一个标量,卡尔曼滤波块假设对角矩阵R,其对角线50和兼容的维度与y。如果测量噪声是高斯,R = 50对应于68%的位置测量下午\ \ sqrt{50} \美元;m美元或实际位置在东部和北部的方向。然而,这种假设并不必要的卡尔曼滤波器。

的元素w美元捕获多少车辆速度可以改变在一个样本时间Ts。过程噪声的方差w, Q矩阵,选择是时变的。它捕获的典型值的直觉w [n]美元速度大时更小。例如,从0到10 m / s比从容易10到20米/秒。具体地说,您使用估计的北部和东部速度和饱和函数构造问[n]:

$ $ f{坐在}(z) = min (max (z, 25), 625) $ $

Q [n]左= \ [$ $ & # xA;开始\{数组}{c c} & # xA;\ displaystyle 1 + \压裂{250}{f{坐在}(\帽子{\点{x}} _e ^ 2)} & # 38;0 xA \ \ & #;0 & # 38;\ displaystyle 1 + \压裂{250}{f{坐在}({\点{x}} \帽子_n”^ 2)} & # xA;结束\{数组}& # xA;\右]& # xA; $ $

Q模型的对角线w成反比的方差估计速度的平方。饱和度函数阻止问变得太大或小。系数250从最小二乘法获得适合0 - 5、5 - 10、10 - 15、15 - 20,20-25m / s为通用汽车加速时间数据。注意,对角问选择代表一个天真的假设速度北部和东部方向的变化是不相关的。

卡尔曼滤波块输入和设置

卡尔曼滤波器的块系统辨识工具箱/估计图书馆在仿真软件。金宝app它还在控制系统工具箱图书馆。配置块参数离散时间状态估计。指定以下过滤器设置参数:

  • 时间域:离散时间。选择这种方式来估计离散时间状态。

  • 选择使用当前测量y [n]改善xhat [n]复选框。这实现了离散卡尔曼滤波器的“当前估计量”变体。这个选项提高估计精度和更有用的样本时间慢。然而,它增加了计算成本。此外,这种卡尔曼滤波器变异直接引线,导致一个代数循环如果卡尔曼滤波器用于反馈回路,不包含任何延误(反馈回路本身也有直接引线)。代数循环可以进一步影响仿真的速度。

单击选项选项卡设置块尺寸和外港选项:

  • 取消添加输入端口u复选框。没有已知输入工厂模型。

  • 选择输出状态估计误差协方差Z复选框。Z矩阵提供的信息过滤器对状态估计的信心。

点击模型参数指定工厂模型和噪声特点:

  • 模型来源:个人A, B, C, D矩阵。

  • 一个A:一个矩阵被定义在这个例子。

  • C:C . C矩阵被定义在这个例子。

  • 初步估计源:对话框

  • 初始状态x [0]:0。这代表了一个初始猜测0的位置和速度估计在t = 0。

  • 状态估计误差协方差P [0]:10。假定你的初始猜测x[0]之间的错误和它的实际价值是一个随机变量的标准偏差$ \ sqrt {10} $。

  • 选择使用G和H矩阵(默认G =我和H = 0)复选框来指定非默认G矩阵。

  • G:G G矩阵被定义在这个例子。

  • H:0。过程噪声不会影响测量y进入卡尔曼滤波器。

  • 取消定常问复选框。Q矩阵是时变和提供块尺寸问:块使用时变卡尔曼滤波器由于此设置。你可以选择这个选项使用定常卡尔曼滤波器。这个问题的定常卡尔曼滤波器性能稍差,但更容易设计和计算成本较低。

  • R:r .这是测量噪声的协方差v [n]美元。在这个例子中早些时候R矩阵的定义。

  • N:0。假设没有过程和测量噪声之间的相关性。

  • 样品时间继承(1)定义:Ts,早在这个例子。

结果

卡尔曼滤波器的性能测试通过模拟一个场景,汽车使以下动作:

  • 在t = 0的车辆美元x_e(0) = 0美元,x_n(0) = 0美元而且是静止的。

  • 向东,它加速到25米/秒。它减慢了50年代在t = 5 m / s。

  • 在t = 100年代,它转向北部和加速到20米/秒。

  • 在t = 200年代,它使另一个转向西方。它加速到25米/秒。

  • 在t = 260年代,它减慢了15米/秒,使恒速180度转弯。

模拟仿真软件模型。金宝app情节的实际、测量和卡尔曼滤波器的估计车辆的位置。

sim卡(“ctrlKalmanNavigationExample”);图;%绘制结果和用实线连接数据点。情节(x (: 1) x (:, 2),“软”,…(:1),y (:, 2),“gd”,…xhat (: 1), xhat (:, 2),“罗”,…“线型”,“- - -”);标题(“位置”);包含(“东[m]”);ylabel (“北[m]”);传奇(“实际”,“测量”,“卡尔曼滤波器估计”,“位置”,“最佳”);轴紧;

之间的误差测量和实际位置之间的误差以及卡尔曼滤波估计和实际位置是:

%东位置测量误差[m]n_xe = y (: 1) - x (: 1);%北位置测量误差[m]n_xn = y (:, 2) - x (:, 2);%卡尔曼滤波器东位置误差[m]e_xe = xhat (: 1) - x (: 1);%卡尔曼滤波器北位置误差[m]e_xn = xhat (:, 2) - x (:, 2);图;%东位置错误次要情节(2,1,1);情节(t, n_xe‘g’t e_xe“r”);ylabel (“位置误差——东[m]”);包含(“时间[s]”);传奇(sprintf (“量:% .3f”规范(n_xe 1) /元素个数(n_xe)), sprintf (“卡尔曼f。:% .3f '规范(e_xe 1) /元素个数(e_xe)));轴紧;%北位置错误次要情节(2,1,2);情节(t、y (:, 2) - x (:, 2),‘g’t xhat (:, 2) - x (:, 2),“r”);ylabel (“位置误差——北[m]”);包含(“时间[s]”);传奇(sprintf (“量:% .3f”规范(n_xn 1) /元素个数(n_xn)), sprintf (“卡尔曼f: % .3f”规范(e_xn 1) /元素个数(e_xn)));轴紧;

情节传说显示位置测量和估计误差($ | | x_e - \帽子{x} _e | | _1美元和$ | | x_n - \帽子{x} _n”| | _1美元规范化的数据点的数量。卡尔曼滤波器估计有大约25%误差小于原始测量数据。

在东方的实际速度方向和卡尔曼滤波器估计如下所示的阴谋。底部图显示了估计误差。

e_ve = xhat (:, 3) - x (:, 3);% (m / s)卡尔曼滤波速度误差e_vn = xhat (:, 4) - x (:, 4);% (m / s)卡尔曼滤波速度误差图;%的速度在东方向及其估计次要情节(2,1,1);情节(t) x (:, 3),“b”t xhat (:, 3),“r”);ylabel (“速度-东(m / s)”);包含(“时间[s]”);传奇(“实际”,卡尔曼滤波器的,“位置”,“最佳”);轴紧;次要情节(2,1,2);%估计误差情节(t, e_ve“r”);ylabel (“速度误差——东(m / s)”);包含(“时间[s]”);传奇(sprintf (“卡尔曼滤波器:% .3f”规范(e_ve 1) /元素个数(e_ve)));轴紧;

错误的传奇情节展示了东方速度估计误差$ | | \点{x} _e - \帽子{\点{x}} _e | | _1美元规范化数据点的数量。

卡尔曼滤波速度估计正确地跟踪实际速度趋势。噪音水平降低车辆以很高的速度旅行时。这是符合Q矩阵的设计。大的两个峰值在t = 50年代和200年代t =。这些车经过时突然减速,一个急转弯,分别。这些瞬间的速度变化大得多比卡尔曼滤波器的预测,是基于它的Q矩阵输入。几个时间步,赶上滤波器估计实际的速度。

总结

你估计车辆的位置和速度在仿真软件中使用卡尔曼滤波器的块。金宝app噪声模型的动力学过程是时变的。你验证了滤波器性能通过模拟各种车辆机动和随机生成的测量噪声。提供的卡尔曼滤波改进的测量位置和速度估计。

bdclose (“ctrlKalmanNavigationExample”);

系统辨识工具箱文档

金宝app

试着用MATLAB仿真软金宝app件,和其他产品下载188bet金宝搏

得到审判现在