此示例显示了如何在Simulink中使用时变卡尔曼滤波器估计线性系统的状态。您可以使用控制系统工具箱库中的卡尔曼滤波器块,根据嘈杂的位置测量值(如GPS传感器测量值)估计地面车辆的位置和速度。卡万博1manbetx尔曼滤波器中的设备模型具有时间特性-不同的噪声特性。
你要估计地面车辆在北和东方向的位置和速度。车辆可以在二维空间中自由移动,不受任何约束。你设计了一个多用途的导航和跟踪系统,可以用于任何物体,而不仅仅是车辆。
和是车辆从原点出发的朝东和朝北位置,车辆方向是否从东到西是车辆的转向角度。是连续时间变量。
Simu万博1manbetxlink模型主要由两部分组成:车辆模型和卡尔曼滤波。下面几节将对此作进一步解释。
开放式系统(“ctrlKalmanNavigationExample”);
履带车辆用简单的点质量模型表示:
其中,车辆状态为:
车辆参数为:
控制输入为:
模型的纵向动力学忽略了轮胎滚动阻力。该模型的横向动力学假设可以瞬时实现所需的转向角,且不考虑偏航惯性矩。
汽车模型是在ctrlKalmanNavigationExample /车辆模型
子系统。Simulink模型包万博1manbetx含两个PI控制器,用于跟踪车辆在行驶过程中所需的方向和速度ctrlKalmanNavigationExample/速度和方向跟踪
子系统。这允许您指定汽车的各种运行条件,并测试卡尔曼滤波器性能。
卡尔曼滤波是一种基于线性模型估计未知变量的算法。这个线性模型描述了估计变量随时间的变化,以响应模型初始条件以及已知和未知的模型输入。在这个例子中,你估计了以下参数/变量:
在哪里
的术语表示速度,而不是导数运算符。是离散时间指数。卡尔曼滤波器中使用的模型形式如下:
在哪里为状态向量,的测量,是过程噪音吗是测量噪声。卡尔曼滤波假设和是方差已知的零均值独立随机变量吗,,.此处,A、G和C矩阵为:
在哪里
A和G的第三行模型东面速度为随机游走:.在现实中,位置是一个连续时间变量,是速度对时间的积分.A和G的第一行表示这个运动学关系的离散近似:A和G的第二行和第四行表示北速度和位置之间的相同关系。
C矩阵表示只有位置测量是可用的。位置传感器,如GPS,以1Hz的采样率提供这些测量。测量噪声的方差, R矩阵,为. 由于R被指定为标量,因此Kalman滤波器块假定矩阵R是对角的,其对角线为50,并且与y的维数兼容。如果测量噪声是高斯噪声,R=50对应于68%的位置测量值在或东、北方向的实际位置。然而,这个假设对于卡尔曼滤波是不必要的。
的元素捕获车辆速度在一个样本时间Ts内的变化。过程噪声的方差w,即Q矩阵,被选择为时变的。它直观地反映了当速度较大时,则较小。例如,从0到10m/s比从10到20m/s容易。具体地说,你使用估计的北、东速度和饱和度函数来构造Q[n]:
Q的对角线模型,w的方差与估计速度的平方成反比。饱和函数可以防止Q变得过大或过小。通过对普通车辆0- 5,5 - 10,10 - 15,15 - 20,20 -25m/s加速度时间数据进行最小二乘法拟合得到系数250。请注意,对角线Q的选择代表了一个幼稚的假设,即在北和东方向的速度变化是不相关的。
“卡尔曼滤波”块在控制系统工具箱
Simulink中的库万博1manbetx。它也在系统辨识工具箱/估计
图书馆。配置离散时间状态估计的块参数。指定以下过滤器设置参数:
时间域:离散时间。选择此选项可估计离散时间状态。
选择使用电流测量y[n]改善xhat[n]复选框。这实现了离散时间卡尔曼滤波器的“当前估计器”变量。这个选项提高了估计精度,对于慢采样时间更有用。然而,它增加了计算成本。此外,这种卡尔曼滤波器具有直接馈通,如果将卡尔曼滤波器用于不包含任何延迟的反馈环路(反馈环路本身也具有直接馈通),则会导致代数环路。代数回路会进一步影响仿真速度。
点击选项选项卡以设置块输入和输出选项:
取消添加输入端口u复选框。在工厂模型中没有已知的输入。
选择输出状态估计误差协方差Z复选框。Z矩阵提供有关过滤器在状态估计中的置信度的信息。
点击模型参数指定厂房型号及噪音特性:
模型来源:个体A, B, C, D矩阵。
一个A矩阵在前面的例子中已经定义。
C:C.本例前面定义了C矩阵。
初始估计源:对话框
初始状态x [0]:0
.这表示在t=0时的位置和速度估计的初始猜测值为0。
状态估计误差协方差P[0]:10
。假设您的初始猜测x[0]与其实际值之间的误差是具有标准偏差的随机变量.
选择使用G和H矩阵(默认G=I和H=0)复选框以指定非默认G矩阵。
GG矩阵在前面的例子中已经定义。
H:0
.过程噪声不影响进入卡尔曼滤波块的测量值。
取消定常问复选框。Q矩阵是时变的,并通过块入口Q提供。由于这个设置,块使用了一个时变卡尔曼滤波器。您可以选择此选项以使用时不变卡尔曼滤波器。定常卡尔曼滤波器在这个问题上的表现略差,但更容易设计,计算成本更低。
R: r,这是测量噪声的协方差.R矩阵在本例前面已定义。
N:0
.假设过程噪声和测量噪声之间没有相关性。
采样时间(-1表示继承): Ts,在本例的前面定义了它。
通过模拟车辆进行以下机动的场景,测试卡尔曼滤波的性能:
t = 0时,车辆的位置,而且是静止的。
它向东加速到25米/秒,在t=50秒时减速到5米/秒。
在t=100秒时,它转向北方并加速到20米/秒。
在t=200秒时,它又向西转弯。它加速到25米/秒。
t = 260s时减速至15m/s,匀速180度转弯。
仿真Simulink模型。万博1manbetx绘制车辆位置的实际、测量和卡尔曼滤波估计。
sim卡(“ctrlKalmanNavigationExample”);图;%绘制结果,用实线连接数据点。情节(x (: 1) x (:, 2),“bx”,...y(:,1),y(:,2),“gd”,...xhat (: 1), xhat (:, 2),“罗”,...“线型”,“- - -”);标题(“位置”);包含('东[米]');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]”); 传奇(斯普林特)('Meas:%.3f'规范(n_xe 1) /元素个数(n_xe)),...斯普林特(“卡尔曼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]”); 传奇(斯普林特)('Meas:%.3f'规范(n_xn 1) /元素个数(n_xn)),...斯普林特('卡尔曼f:%.3f'规范(e_xn 1) /元素个数(e_xn)));轴紧的;
图例显示位置测量和估计误差(和),按数据点的数量归一化。卡尔曼滤波估计的误差大约比原始测量少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 (“速度-东[米/秒]”);包含(“时间[s]”);传奇(“实际的”,“卡尔曼滤波器”,“位置”,“最佳”);轴紧的;次要情节(2,1,2);%估计误差图(t、e、e),“r”);ylabel (“速度误差-东面[m/s]”);包含(“时间[s]”); 传奇(斯普林特)('卡尔曼滤波器:%.3f'规范(e_ve 1) /元素个数(e_ve)));轴紧的;
误差图上的图例显示了东速度估计误差根据数据点的数量归一化。
卡尔曼滤波速度估计正确地跟踪了实际速度趋势。汽车在高速行驶时噪音会降低。这符合Q矩阵的设计。大的两个峰值在t=50和t=200时。这些分别是汽车突然减速和急转弯的时间。这些时刻的速度变化比卡尔曼滤波器的预测要大得多,卡尔曼滤波器是基于其Q矩阵输入的。经过几个时间步后,滤波器估计的速度就会赶上实际速度。
您使用Simulink中的卡尔曼滤波块估计了车辆的位置和速度。模型的过程噪声动态是时变的。您通过模拟各种车辆操纵和随机生成的测量噪声来验证过滤器性能。卡尔曼滤波器改进了位置万博1manbetx测量并提供了车辆的速度估计。
bdclose (“ctrlKalmanNavigationExample”);