卡尔曼定位程序

卡尔曼定位程序.zip

function ekf_for_track_9
clc;
clear;
n=9;
T=0.5;        % 雷达扫描周期
N=50;         % 总的采样次数

F=[100T00T^2/200;
   0100T00T^2/20;
   00100T00T^2/2;
   000100T00;
   0000100T0;
   00000100T;
   000000100;
   000000010;
   000000001]; % 状态转移矩阵

Q=[1 0 0 0 0 0 0 0 0;    % 过程噪声协方差矩阵
    0 1 0 0 0 0 0 0 0;
    0 0 1 0 0 0 0 0 0;
    0 0 0 0.01 0 0 0 0 0;
    0 0 0 0 0.01 0 0 0 0;
    0 0 0 0 0 0.01 0 0 0;
    0 0 0 0 0 0 0.0001 0 0;
    0 0 0 0 0 0 0 0.0001 0;
    0 0 0 0 0 0 0 0 0.0001];

% R=0.1*pi/180;   % 观测噪声方差（因为只有一个观测，所以是一个值，不是协方差矩阵）
R = [100 0 0;             % 观测噪声协方差矩阵  
    0 0.001^2 0
    0 0 0.001^2];

X=zeros（9N）;   % 目标真实位置、速度
X（:1）=[1000;5000;200;10;50;10;2;-4;2]+sqrtm（Q）*randn（n1）;

Z=zeros（3N）;   % 传感器对位置的观测

x0=0;           
y0=0; 
z0=0;
Xstation=[x0;y0;z0];   % 观测站的位置
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for t=2:N
    X（:t）=F*X（:t-1）+sqrtm（Q）*randn（91）;  % 目标真实轨迹
end

for t=1:N
    % Z（t）=hfun（X（:t）Xstation）+w（t）;          % 对目标的观测
    [ddalphabeta]=funh（X（:t）Xstation）;
    Z（:t） = [ddalphabeta]‘ + sqrtm（R）*randn（31）;
end

% EKF滤波
Xekf=zeros（9N）;   % EKF滤波值
Xekf（:1）=X（:1）;  % EKF滤波初始化
P0 =[100 0 0 0 0 0 0 0 0;             % 协方差初始化
     0 100 0 0 0 0 0 0 0;
     0 0 100 0 0 0 0 0 0;
     0 0 0 1 0 0 0 0 0;
     0 0 0 0 1 0 0 0 0;
     0 0 0 0 0 1 0 0 0;
     0 0 0 0 0 0 0.1 0 0;
     0 0 0 0 0 0 0 0.1 0
     0 0 0 0 0 0 0 0 0.1];
 
for i=2:N
    Xn=F*Xekf（:i-1）;            % 预测
    P1=F*P0*F‘+ Q;               % 预测误差协方差   没有G就不写
    
    [ddalphabeta]=funh（XnXstation）;   % 观测预测
    % 求Jacobian矩阵H，H为3行9列的矩阵
    D = Dist（XnXstation）;
    DD = Dist3（XnXstation）;
    H = [（Xn（11）-x0）/DD（Xn（21）-y0）/DD（Xn（31）-z0）/DD000000;
          -（Xn（21）-y0）/D^2（Xn（11）-x0）/D^20000000;
          （1/（1+（（Xn（31）-z0）/D）^2））.*（-2*（Xn（11）-x0）/D^4）（1/（1+（（Xn（31）-z0）/D）^2））.*（-2*（Xn（21）-y0）/D^4）（1/D）/（1/（1+（（Xn（31）-z0）/D）^2））000000];
  
    K=P1*H‘*inv（H*P1*H‘+R）;                    % kalman增益
    Xekf（:i）=Xn+K*（Z（:i）-[ddalphabeta]‘）;  % 状态更新
    P0=（eye（9）-K*H）*P1;                        % 协方差更新
end

Err_KalmanFilter = zeros（1N）;
for i=1:N
  Err_KalmanFilter（i）= （Dist3（X（:i）Xekf（:i）））;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 画图，轨迹图
figure
t=1:N;
hold on;
box on;
grid on;
plot3（X（1t）X（2t）X（3t）‘k-‘）;
plot3（Z（1t）.*cos（Z（3t））.*cos（Z（2t））Z（1t）.*cos（Z（3t））.*sin（Z（2t））Z（1t）.*sin（Z（3t））‘-b.‘）;
plot3（Xekf（1t）Xekf（2t）Xekf（3t）‘-r.‘）;
legend（‘轨迹‘‘估计点‘‘实际点‘）;
xlabel（‘x方向位置/米‘）
ylabel（‘y方向位置/米‘）
zlabel（‘z方向位置/米‘）
view（3）


figure
hold on;
box on;
plot（Err_KalmanFilter‘-ks‘‘MarkerFace‘‘r‘）;
legend（‘跟踪误差‘）;

%figure 
%hold on;
%box on;
%plot（Z/pi*180‘-r.‘‘MarkerFace‘‘r‘）;
%plot（Z/pi*180+w/pi*180‘-ko‘‘MarkerFace‘‘g‘）;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2020-09-08 15:07  卡尔曼定位程序\
     目录           0  2018-01-02 21:15  卡尔曼定位程序\SI\
     文件         178  2017-11-06 11:06  卡尔曼定位程序\SI\RMS.m
     文件        1107  2017-11-19 11:50  卡尔曼定位程序\SI\SI.m
     文件        1727  2017-11-17 19:42  卡尔曼定位程序\SI\main.m
     文件         593  2020-09-08 14:07  卡尔曼定位程序\TOALLOP.m
     目录           0  2018-01-02 21:15  卡尔曼定位程序\Taylor\
     文件         178  2017-11-06 11:06  卡尔曼定位程序\Taylor\RMS.m
     文件        1342  2017-11-19 12:07  卡尔曼定位程序\Taylor\Taylor.m
     文件        1730  2017-11-17 19:57  卡尔曼定位程序\Taylor\main.m
     目录           0  2018-01-02 21:15  卡尔曼定位程序\chan\
     文件         178  2017-11-06 11:06  卡尔曼定位程序\chan\RMS.m
     文件        1690  2017-11-19 10:34  卡尔曼定位程序\chan\main.m
     文件        2522  2017-11-19 11:04  卡尔曼定位程序\chan\zz.m
     文件        4680  2020-09-11 20:11  卡尔曼定位程序\ekf_for_track_9.m
     目录           0  2018-01-02 21:15  卡尔曼定位程序\fang\
     文件        1109  2017-11-19 11:30  卡尔曼定位程序\fang\Fang.m
     文件         178  2017-11-06 11:06  卡尔曼定位程序\fang\RMS.m
     文件        1723  2017-11-17 19:23  卡尔曼定位程序\fang\main.m
     文件        1996  2020-09-08 14:08  卡尔曼定位程序\main.m