物体的运动轨迹预测扩展卡尔曼滤波算法实现

运动物体的轨迹预测，分别使用卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波以及数据拟合方法实现。本例代码仅含扩展卡尔曼滤波部分代码。 本例仅为本人在研究轨迹预测问题时为理解算法原理所写，针对具体问题请自行斟酌算法适用性。 本例代码详解后续会在本人博客中做具体说明，欢迎讨论！

%使用卡尔曼滤波方法对飞行航班进行轨迹预测
%数据来源：FlightAware（https://zh.flightaware.com）
%航    班：CES9937     宁波栎社国际机场飞往成都双流国际机场
%飞行时间：2018-07-17  07:07-09:55
%说明：取起飞后前20组数据作为实验数据。对时间点进行近似取值，假设每隔30s上报一次数据
clear;
clc;
%采样点的个数
N=228;
%测试数据：纬度
latitude=load（‘C:\workspace\matlab workspace\CES9937\latitude.txt‘）;
%系统噪声方差
Q=0.25;
%观测噪声方差
R=0.01;
%产生系统噪声
W=sqrt（Q）*randn（1N）;
%产生测量噪声
V=sqrt（R）*randn（1N）;
%定义符号变量t
syms t;
%方程-根据二阶拟合，纬度关于时间t的函数
%lat = 0.000129*t^2 - 0.022433*t + 29.819;
%方程-根据三阶拟合，纬度关于时间t的函数
%lat = （-1.4341e-06）*t^3 + （0.00062162）*t^2 + （-0.067656）*t +  30.691;
%方程-根据六阶拟合，纬度关于时间t的函数
lat = （2.5716e-12）*t^6 + （-1.5634e-09）*t^5 + （3.2239e-07）*t^4 + （-2.427e-05）*t^3 + （0.00037919）*t^2 + （-0.0017116）*t + 29.728;
%一阶泰勒展开
%lat_taylor = taylor（latt1）
%求解雅克比矩阵作为状态转移矩阵
lat_jacobian = jacobian（latt）;
%观测矩阵
H = 1;
%根据状态方程，计算纬度lat_x
lat_x = zeros（1N）;
%设置初始值
lat_x（1） = 29.8000;
%在实验中，假设观测值就是真实值
for k=2:N
    lat_x（k） = latitude（k）;
end
%EKF滤波算法
%卡尔曼对纬度的滤波结果
lat_ekf = zeros（1N）;
%假设滤波初始值和假设的初始值相同
lat_ekf（1） = lat_x（1）;
%协方差矩阵初始值
P0 = eye（1）;
%观测值
lat_obse = zeros（1N）;
%设置观测初值
lat_obse（1） = lat_x（1） + V（1）;
for k = 2:N
    %系统状态方程求取理论值
    lat_theo = subs（lattk）;
    lat_obse（k） = lat_theo + V（k）;
    %状态转移矩阵F
    F = subs（lat_jacobiantk）;
    %协方差预测
    P = F*P0*F‘ + Q;
    %求卡尔曼增益
    K = P*H‘*inv（H*P*H‘ + R）;
    %状态更新
    lat_ekf（k） = lat_theo + K*（lat_x（k） - lat_obse（k））;
    %协方差阵更新
    P0 = （eye（1） - K*H）*P;
end
%误差分析
%滤波与真实值之间的偏差
Err_Kalman = zeros（1N）;
%测量值与真实值之间的偏差
Err_Messure=zeros（1N）;

for k=1:N
    Err_Kalman（k） = abs（lat_ekf（k） - lat_x（k））;
    Err_Messure（k） = abs（lat_obse（k） - lat_x（k））;
end
%滤波效果图
t = 1:N;
figure
hold on;
box on;
plot（tlat_x‘g-‘tlat_obse‘b-‘tlat_ekf‘r-‘）;
legend（‘真实值‘‘观测值‘‘kalman滤波值‘）;
xlabel（‘测报时间点‘）;
ylabel（‘纬度值‘）;
%误差分析图
figure
hold on;
box on;
plot（tErr_Kalman‘r-‘）;
legend（‘kalman滤波偏差‘）;
xlabel（‘测报时间点‘）;
ylabel（‘纬度偏差值‘）;
%误差分析图
figure
hold on;
box on;
plot（tErr_Messure‘b-‘）;
legend（‘系统状态方程偏差‘）;
xlabel（‘测报时间点‘）;
ylabel（‘纬度偏差值‘）;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       2523  2018-08-02 10:42  QA_EKF_lat.m

     文件     153993  2018-07-24 16:28  CES9937\CES9937.docx

     文件       1825  2018-07-17 15:07  CES9937\latitude.txt

     文件       2051  2018-07-17 15:15  CES9937\longitude.txt

     文件        410  2018-07-17 11:16  CES9937\飞行数据-20组.txt

     目录          0  2018-08-14 20:18  CES9937

----------- ---------  ---------- -----  ----

               160802                    6