主要内容

基于时变卡尔曼滤波的状态估计

这个示例使用:

打开脚本

这个例子展示了如何在Simulink中使用时变卡尔曼滤波器估计线性系统的状态。金宝app您可以使用控制系统工具箱库中的卡尔曼滤波块来估计地面车辆的位置和速度，基于噪声位置测量，如GPS传感器测量。卡尔曼滤波中的对象模型具有时变噪声特性。

介绍

你要估计地面车辆在北和东方向的位置和速度。车辆可以在二维空间中自由移动，不受任何约束。你设计了一个多用途的导航和跟踪系统，可以用于任何物体，而不仅仅是车辆。

美元美元x_e (t)和x_n (t)美元是车辆从原点出发的朝东和朝北位置，\θ(t)美元车辆的方向是从东方和u_ \ psi (t)美元是车辆的转向角度。元新台币为连续时间变量。

Simu金宝applink模型主要由两部分组成:车辆模型和卡尔曼滤波。下面几节将对此作进一步解释。

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)} - A \;重金属镉\; s(t)^2) / m \\ s(t) \tan(u_\psi(t)) / L \end{array} \right] $$

其中，车辆状态为:

$$ \begin{array} {all} 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} $$

车辆参数为:

$$ \begin{array} {ll} 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} $$

控制输入为:

$$ \begin{array} {ll} u_T(t) \;& # 38;\textnormal{油门位置在-1和1的范围内}\;(无单位)\ \ & # xA; u_ \ psi (t) \;& # 38;\ textnormal{转向角}\;[度]\ \ & # xA; \{数组}$ $

模型的纵向动力学忽略了轮胎滚动阻力。该模型的横向动力学假设可以瞬时实现所需的转向角，且不考虑偏航惯性矩。

汽车模型是在ctrlKalmanNavigationExample /车辆模型子系统。Simu金宝applink模型包含两个PI控制器，用于跟踪汽车的期望方向和速度ctrlKalmanNavigationExample/速度和方向跟踪子系统。这允许您指定汽车的各种运行条件，并测试卡尔曼滤波器性能。

卡尔曼滤波器设计

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

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

在哪里

$$ \begin{array} {all} \hat{x}_e[n] \;& # 38;\textnormal{East position estimate} \;[m] \ \ & # xA; \帽子{x} _n”[n] \;& # 38;\textnormal{北位置估计}\;[m] \ \ & # xA; \帽子{\点{x}} _e [n] \;& # 38;\textnormal{East velocity estimate} \;(米/秒)\ \ & # 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矩阵是:

$$ A = \left[ \begin{array}{c c c} 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 = \left[ 开始\{数组}{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 = \left[ \begin{array}{c c c} 1 & # 38岁;0 & # 38;0 & # 38;0 xA \ \ & #;0 & # 38;1 & # 38岁;0 & # 38;0 xA & #; \end{array} \right] $$

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

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

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

的元素w美元捕获车辆速度在一个样本时间Ts内的变化。过程噪声的方差w，即Q矩阵，被选择为时变的。它直观地反映了w [n]美元当速度较大时，则较小。例如，从0到10m/s比从10到20m/s容易。具体地说，你使用估计的北、东速度和饱和度函数来构造Q[n]:

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

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

Q的对角线模型，w的方差与估计速度的平方成反比。饱和函数可以防止Q变得过大或过小。通过对普通车辆0- 5,5 - 10,10 - 15,15 - 20,20 -25m/s加速度时间数据进行最小二乘法拟合得到系数250。请注意，对角线Q的选择代表了一个幼稚的假设，即在北和东方向的速度变化是不相关的。

卡尔曼滤波块输入和设置

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

  • 时间域:离散时间。选择这个选项来估计离散时间状态。

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

单击选项选项卡来设置块的输入和输出选项:

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

  • 选择输出状态估计误差协方差Z复选框。Z矩阵提供了关于状态估计的滤波器置信度的信息。

点击模型参数指定厂房型号及噪音特性:

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

  • 一个A矩阵在前面的例子中已经定义。

  • CC矩阵在这个例子的前面已经定义。

  • 初步估计源:对话框

  • 初始状态x [0]：0。这表示在t=0时的位置和速度估计的初始猜测值为0。

  • 状态估计误差协方差P[0]：10。假设您的初始猜测x[0]与其实际值之间的误差是一个带有标准差的随机变量$ \ sqrt {10} $。

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

  • GG矩阵在前面的例子中已经定义。

  • H：0。过程噪声不影响进入卡尔曼滤波块的测量值。

  • 取消定常问复选框。Q矩阵是时变的，并通过块入口Q提供。由于这个设置，块使用了一个时变卡尔曼滤波器。您可以选择此选项以使用时不变卡尔曼滤波器。定常卡尔曼滤波器在这个问题上的表现略差，但更容易设计，计算成本更低。

  • R: r，这是测量噪声的协方差v [n]美元。R矩阵在这个例子的前面已经定义。

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

  • 样本时间(继承时为-1): Ts，在本例的前面定义了它。

结果

通过模拟车辆进行以下机动的场景，测试卡尔曼滤波的性能:

  • t = 0时，车辆的位置美元x_e(0) = 0美元，x_n(0) = 0美元而且是静止的。

  • 向东，它加速到每秒25米。在t=50s时减速到5m/s。

  • 在t = 100时，转向北，加速到20m/s。

  • 在t = 200时，它又向西转向。它加速到25m/s。

  • t = 260s时减速至15m/s，匀速180度转弯。

仿真Simulink模型。金宝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]”）;传奇(“实际”，卡尔曼滤波器的，“位置”，“最佳”）;轴紧；次要情节(2,1,2);%估计误差情节(t, e_ve“r”）;ylabel (“速度误差-东[m]”）;包含(“时间[s]”）;传奇(sprintf (“卡尔曼滤波器:% .3f”规范(e_ve 1) /元素个数(e_ve)));轴紧；

误差图上的图例显示了东速度估计误差$ | | \点{x} _e - \帽子{\点{x}} _e | | _1美元根据数据点的数量归一化。

卡尔曼滤波速度估计正确地跟踪了实际速度趋势。汽车在高速行驶时噪音会降低。这符合Q矩阵的设计。大的两个峰值在t=50和t=200时。这些分别是汽车突然减速和急转弯的时间。这些时刻的速度变化比卡尔曼滤波器的预测要大得多，卡尔曼滤波器是基于其Q矩阵输入的。经过几个时间步后，滤波器估计的速度就会赶上实际速度。

总结

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

bdclose (“ctrlKalmanNavigationExample”）;

控制系统工具箱文档

金宝app

学习如何自动调整PID控制器增益

学习如何自动调整PID控制器增益

下载代码示例