function OriginalDataTester()
global T;%采样周期
T=1;
F =[1,T,T^2,0,0,0;...
0,1,T,0,0,0;...
0,0,1,0,0,0;...
0,0,0,1,T,T^2;...
0,0,0,0,1,T;...
0,0,0,0,0,1];
H=[1,0,0,0,0,0;...
0,0,0,1,0,0];
Polar_Z0 = xlsread('F:\新型管制自动化系统核心技术\雷达真实数据\test00_01.xls',2,'E2:F4');
Z0 = zeros(3,2);
for i = 1:3
[R,A] = transfer2cartestain(Polar_Z0(i,1),Polar_Z0(i,2));
Z0(i,:) = [R A];
end
S_ = [0 0 1; 0 -1 1;1 -2 1]*Z0;
S0 = [S_*[1;0];S_*[0;1]];
S = S0;
S_K = S0;
N = 100;
Location = zeros(2,422);
Location_K = zeros(2,422);
Velocity = zeros(2,422);
Velocity_K = zeros(2,422);
Ascendation = zeros(2,422);
Ascendation_K = zeros(2,422);
Z = xlsread('F:\新型管制自动化系统核心技术\雷达真实数据\test00_01.xls',2,'E5:F426');
for i = i:N
S = S0;
S_K = S0;
% 设置噪声水平
%极坐标下的观测噪声协方差矩阵，要修改极坐标系观测噪声水平就修改var_R，var_A即可
var_R= 80 * randn(1);
var_A=0.0001*randn(1);
R_ForBlue = [var_R,0;...
0,var_A];
%笛卡尔坐标下的观测噪声协方差矩阵，要修改笛卡尔坐标下的噪声水平，仅修改var_x,var_y即可
var_x = 5*randn(1);
var_y = 5*randn(1);
var_x2 = var_x^2;
var_y2 = var_y^2;
R_ForKalman =[var_x2,0;...
0,var_y2];
%过程噪声协方差矩阵（kalman和Blue都要用到）
PNL = 10*randn(1);%要修改过程噪声（模型噪声）水平的话就修改这个数字。
Q=zeros(6,6);
for i=1:6
Q(i,i) = PNL^2;
end
% 初始的状态协方差矩阵
P = Q;
P_K = P;
for i=1:422
z = Z(i,1:2);
[S,P]=MyKarlman_Blue(S,P,F,Q,z,H,R_ForBlue);
[x,y] = transfer2cartestain(Z(i,1),Z(i,2));
z_k = [x; y];
[S_K,P_K]=MyKalman(S_K,P_K,F,Q,z_k,H,R_ForKalman);
Location(:,i)=Location(:,i) + [S(1);S(4)];
Velocity(:,i) = Velocity(:,i) + [S(2);S(5)];
Ascendation(:,i) = Ascendation(:,i) + [S(3);S(6)];
Location_K(:,i)=Location_K(:,i) + [S_K(1);S_K(4)];
Velocity_K(:,i) = Velocity_K(:,i) + [S_K(2);S_K(5)];
Ascendation_K(:,i) = Ascendation_K(:,i) + [S_K(3);S_K(6)];
end
end
Location = Location/N;
Velocity = Velocity/N;
Ascendation = Ascendation/N;
Location_K = Location_K/N;
Velocity_K = Velocity_K/N;
Ascendation_K = Ascendation_K/N;
figure;
plot(Location(1,:),Location(2,:),'r',Location_K(1,:),Location_K(2,:),'g');
title('位置');
xlabel('X (m)');
ylabel('Y (m)');
% axis equal;
figure;
plot(Velocity(1,:),Velocity(2,:),'r',Velocity_K(1,:),Velocity_K(2,:),'g');
title('速度');
xlabel('X(m/s)');
ylabel('Y (m/s');
% axis equal;
figure;
plot(Ascendation(1,:),Ascendation(2,:),'r',Ascendation_K(1,:),Ascendation_K(2,:),'g');
title('加速度');
xlabel('X (m/s^2)');
ylabel('Y (m/s^2)');
%@subfunction
%把极坐标下的量测转化为笛卡尔坐标系下
function [x,y]=transfer2cartestain(R,A)
%parameter:
% R:极径
% A:极角
x = R*cos(A/180*pi);
y = R*sin(A/180*pi);
%@subfunction
%把笛卡尔坐标转化极坐标
function [R,A]=transfer2Polar(x,y)
%parameter:
% R:极径
% A:极角
if (x == 0 && y > 0)
A = 1/2*pi;
else if (x > 0 && y == 0)
A = 0;
else if (x > 0 && y > 0)
A = atan(y/x);
else if (x < 0 && y > 0)
A = atan(y/x) + pi;
else if (x < 0 && y == 0)
A = pi;
else if (x < 0 && y < 0)
A = atan(y/x) + pi;
else if (x == 0 && y < 0)
A = 3/2*pi;
else if (x > 0 && y < 0)
A = atan(y/x) + 2*pi;
end
end
end
end
end
end
end
end
R = sqrt(x^2 + y^2);
- 1
- 2
- 3
前往页