卡尔曼滤波用于单目标定位的程序

此程序是用于单目标的跟踪定位代码，系统选用匀加速的情况，代码没有bug，可直接运行出结果，比较了运用KF前后的RMSE均方根误差，有图形表示，适合初学者学习Kalman Filter。 用得好，麻烦好评一下，谢谢嘻嘻嘻！

%  Kalman滤波在船舶GPS导航定位系统中的应用
function main
clc;clear;
T=1;%雷达扫描周期
N=80/T;%总的采样次数
X=zeros（4N）;%目标真实位置、速度
X（:1）=[-100220020];%目标初始位置、速度
Z=zeros（2N）;%传感器对位置的观测
Z（:1）=[X（11）X（31）];%观测初始化
delta_w=1e-2;%如果增大这个参数，目标真实轨迹就是曲线
Q=delta_w*diag（[0.510.51]） ;%过程噪声均值
R=100*eye（2）;%观测噪声均值
F=[1T00;0100;001T;0001];%状态转移矩阵
H=[1000;0010];%观测矩阵
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for t=2:N
    X（:t）=F*X（:t-1）+sqrtm（Q）*randn（41）;%目标真实轨迹
    Z（:t）=H*X（:t）+sqrtm（R）*randn（21）; %对目标观测
end
%卡尔曼滤波
Xkf=zeros（4N）;
Xkf（:1）=X（:1）;%卡尔曼滤波状态初始化
P0=eye（4）;%协方差阵初始化
for i=2:N
    Xn=F*Xkf（:i-1）;%预测
    P1=F*P0*F‘+Q;%预测误差协方差
    K=P1*H‘*inv（H*P1*H‘+R）;%增益
    Xkf（:i）=Xn+K*（Z（:i）-H*Xn）;%状态更新
    P0=（eye（4）-K*H）*P1;%滤波误差协方差更新
end
%误差更新
for i=1:N
    Err_Observation（i）=RMS（X（:i）Z（:i））;%滤波前的误差
    Err_KalmanFilter（i）=RMS（X（:i）Xkf（:i））;%滤波后的误差
end
%画图
figure
hold on;box on;
plot（X（1:）X（3:）‘-k‘）;%真实轨迹
plot（Z（1:）Z（2:）‘-b.‘）;%观测轨迹
plot（Xkf（1:）Xkf（3:）‘-r+‘）;%卡尔曼滤波轨迹
legend（‘真实轨迹‘‘观测轨迹‘‘滤波轨迹‘）
figure
hold on; box on;
plot（Err_Observation‘-ko‘‘MarkerFace‘‘g‘）
plot（Err_KalmanFilter‘-ks‘‘MarkerFace‘‘r‘）
legend（‘滤波前误差‘‘滤波后误差‘）
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%计算欧式距离子函数
function dist=RMS（X1X2）;
if length（X2）<=2
    dist=sqrt（ （X1（1）-X2（1））^2 + （X1（3）-X2（2））^2 ）;
else
    dist=sqrt（ （X1（1）-X2（1））^2 + （X1（3）-X2（3））^2 ）;
end
%%%%%%%%%%%%%%%%%%%%%%%%

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2018-12-21 16:31  KF Boat_ZKX\
     文件        1656  2018-12-19 20:12  KF Boat_ZKX\BoatCSDN.m
     文件       26240  2018-12-21 16:31  KF Boat_ZKX\Error Analysis.fig
     文件       34517  2018-12-21 16:31  KF Boat_ZKX\KF Boat Tracking.fig
     文件        2485  2018-12-21 16:31  KF Boat_ZKX\KF_BoatLocation_ZKX.asv
     文件        2492  2018-12-21 16:31  KF Boat_ZKX\KF_BoatLocation_ZKX.m
     文件          73  2018-12-19 20:17  KF Boat_ZKX\RMSE.m