关于对于车辆轨迹的卡尔曼滤波仿真以及自己对于卡尔曼滤波的理解
时间:2022-09-21 13:30:00
1. 理论
最近,我学习了卡尔曼滤波器。由于卡尔曼滤波器的公式非常简单,最终公式仅发布在这里。它是由贝叶斯滤波器推出的,假设每个变量都是高斯分布和独立的。公式如下:
上述公式为卡尔曼滤波器(KF)扩展卡尔曼滤波器的公式无非是状态转移矩阵的方程是非线性的。我们只针对非线性局部线性化,优雅可比矩阵,然后根据卡尔曼滤波器公式计算。
2. 无人机轨迹预测和模拟 (卡尔曼滤波)
2.1 理解卡尔曼滤波
一般来说,模拟卡尔曼滤波器是在已知状态方程的情况下获得每个矩阵的值。首先计算高斯噪声的测量值。在获得测量值后,您可以首先初始化卡尔曼公式,然后进行预测,以获得此时的预测值。预测后,获得此时的更新值,即后验估计,然后依次迭代,最后绘制波形。
事实上,卡尔曼滤波器在真实场景中的观测值可以通过传感器直接获得。根据传感器和状态方程间接获得预测值。
在PX4源码中就是根据IMU根据磁罗盘等其他传感器,间接计算数据来预测我们想要的位置和其他状态,GPS,外部视觉或理论推导得到我们的观测值,然后通过扩展卡尔曼滤波得到我们想要的状态。详见官方文件PX扩展卡尔曼滤波EKF的应用
2.2 轨迹仿真
(使用matlab2019进行仿真)
%%%扩展卡尔曼滤波模拟 close all; clear; clc; rng(1000)%以后会产生随机种子rand会用到 %% 参数 Delta_t = 0.1; %多传感器采样时间短板采样时间 t_max = 100; %总采样时间 k_max = t_max / Delta_t;%卡尔曼滤波多少次? V_c = 20; %汽车的初始速度 omega_c = 0; %车的初始角速 H = [1 0 0; 0 1 0; 0 0 1];%观测和输入矩阵被视为线性矩阵 Q = [Delta_t 0 0; 0 Delta_t 0; 0 0 Delta_t/180];%协方差矩阵预测噪声 R = [100 0 0; 0 100 0; 0 0 0.01];%协方差矩阵观察噪声 %% 初始化 m_0 = zeros(3, 1);%x_0的期望 P_0 = [1e6
0
0
;
0
1e6
0
;
0
0 pi
^
2
]
;
%x_0的协方差 x_0
=
mvnrnd
(m_0
, P_0
)'
;
%产生多元正态随机数 x_0是预测的三个变量的初始值随机生成的
%
% 仿真数据初始化 xSequence
=
zeros
(
3
, k_max
+
1
)
;
%预测的初始序列 ySequence
=
zeros
(
3
, k_max
+
1
)
;
%观测的初始序列 x_hatSequence
=
zeros
(
3
, k_max
+
1
)
;
%后验的初始序列 k_indexC
=
1
;
%用于数组下标补偿 kSequence
=
0
: k_max
;
for k
= kSequence
%
% 得到测量值
if k
==
0
xSequence
(
:
, k
+k_indexC
)
= x_0
;
else
% k
>
0
xSequence
(
1
, k
+k_indexC
)
=
xSequence
(
1
, k
-
1
+k_indexC
)
+ V_c
* Delta_t
*
cos
(
xSequence
(
3
, k
-
1
+k_indexC
)
)
;
xSequence
(
2
, k
+k_indexC
)
=
xSequence
(
2
, k
-
1
+k_indexC
)
+ V_c
* Delta_t
*
sin
(
xSequence
(
3
, k
-
1
+k_indexC
)
)
;
xSequence
(
3
, k
+k_indexC
)
=
xSequence
(
3
, k
-
1
+k_indexC
)
+ omega_c
* Delta_t
;
xSequence
(
:
, k
+k_indexC
)
=
xSequence
(
:
, k
+k_indexC
)
+
mvnrnd
(
zeros
(
3
,
1
)
, Q
)'
;
% Add process
/dynamicial noise end
%上面的操作主要是得到这里的测量值 xSequence是预测值 ySequence是得到的虚拟的测量值,实际中就是传感器的采样值(GPS)
ySequence
(
:
, k
+k_indexC
)
= H
*
xSequence
(
:
, k
+k_indexC
)
+
mvnrnd
(
zeros
(
3
,
1
)
, R
)'
;
%
% 状态估计
%
% 预测步
if k
==
0 m_k_prior
= m_0
;
%初始期望 P_k_prior
= P_0
;
%初始方差
else
% k
>
0
%首先得到预测的雅可比矩阵 主要针对非线性 F_k_x
=
[
1
0
-V_c
*Delta_t
*
sin
(
m_kminus1_posterior
(
3
)
)
;
0
1 V_c
*Delta_t
*
cos
(
m_kminus1_posterior
(
3
)
)
;
0
0
1
]
;
% Jacobian matrix of f_k
%依次 得到预测的点的期望 这里期望就代表状态,因为这是卡尔曼滤波
m_k_prior
(
1
)
=
m_kminus1_posterior
(
1
)
+ V_c
* Delta_t
*
cos
(
m_kminus1_posterior
(
3
)
)
;
m_k_prior
(
2
)
=
m_kminus1_posterior
(
2
)
+ V_c
* Delta_t
*
sin
(
m_kminus1_posterior
(
3
)
)
;
m_k_prior
(
3
)
=
m_kminus1_posterior
(
3
)
+ omega_c
* Delta_t
;
%依次 得到预测的点的方差 这里期望就代表状态,因为这是卡尔曼滤波 P_k_prior
= F_k_x
* P_kminus1_posterior
* F_k_x'
+ Q
; end
%
% 更新步
%测量残差协方差 S_k
= H
* P_k_prior
* H'
+ R
;
%卡尔曼增益 K_k
= P_k_prior
* H'
/ S_k
; m_k_posterior
= m_k_prior
+ K_k
*
(
ySequence
(
:
, k
+k_indexC
)
- H
* m_k_prior
)
;
%更新的期望 最终状态 P_k_posterior
= P_k_prior
- K_k
* S_k
* K_k'
;
%更新的协方差 P_k_posterior
=
(P_k_posterior
+ P_k_posterior'
)
/
2
;
%
x_hatSequence
(
:
, k
+k_indexC
)
= m_k_posterior
;
%
%用于递归 m_kminus1_posterior
= m_k_posterior
; P_kminus1_posterior
= P_k_posterior
; end
%
% 图形化表示 tSequence
= Delta_t
* kSequence
; figure
,
plot
(tSequence
,
xSequence
(
1
,
:
)
, tSequence
,
x_hatSequence
(
1
,
:
)
,
'LineWidth'
,
0.5
)
xlabel
(
'Time (sec.)'
)
ylabel
(
'Locations (m)'
)
title
(
'x^{(1)}-axis'
)
legend
(
'x^{(1)}'
,
'Estimate of x^{(1)}'
) grid on
; figure
,
plot
(tSequence
,
xSequence
(
2
,
:
)
, tSequence
,
x_hatSequence
(
2
,
:
)
,
'LineWidth'
,
0.5
)
xlabel
(
'Time (sec.)'
)
ylabel
(
'Locations (m)'
)
title
(
'x^{(2)}-axis'
)
legend
(
'x^{(2)}'
,
'Estimate of x^{(2)}'
) grid on
; figure
,
plot
(tSequence
,
xSequence
(
3
,
:
)
, tSequence
,
x_hatSequence
(
3
,
:
)
,
'LineWidth'
,
0.5
)
xlabel
(
'Time (sec.)'
)
ylabel
(
'Angle (rad)'
)
title
(
'\theta'
)
legend
(
'\theta'
,
'Estimate of \theta'
) grid on
; figure
,
plot
(
xSequence
(
1
,
:
)
,
xSequence
(
2
,
:
)
,
x_hatSequence
(
1
,
:
)
,
x_hatSequence
(
2
,
:
)
,
'LineWidth'
,
0.5
)
xlabel
(
'x^{(1)}-axis (m)'
)
ylabel
(
'x^{(2)}-axis (m)'
)
legend
(
'Trajectory'
,
'Estimate'
) grid on
;
3. 仿真结果
3.1 x轴原始值与预测值
可以看出预测结果基本一致,当然这也是建立在我们仿真阶段,因为仿真我们对于位移的预测非常准确,其预测的方差很小,观测的方差很大,导致最终结果很相信预测,所以主观看起来非常准确。
3.2 y轴原始值与预测值
可以看出预测结果基本一致。
3.3 转向角原始值与预测值
角度的预测结果稍微差一些,主要是因为不仅预测的方差很小,观测的方差也很小,这样一折中,导致误差大了起来。不过整体结果还不错,趋势上差别不大。