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 转向角原始值与预测值
角度的预测结果稍微差一些，主要是因为不仅预测的方差很小，观测的方差也很小，这样一折中，导致误差大了起来。不过整体结果还不错，趋势上差别不大。