Skip to content

Commit

Permalink
[Least Squares]Radar System Error Estimation
Browse files Browse the repository at this point in the history
  • Loading branch information
@qingyuanxingsi
qingyuanxingsi committed Jul 23, 2015
1 parent 235e3b9 commit 27dc200
Show file tree
Hide file tree
Showing 10 changed files with 668 additions and 36 deletions.
108 changes: 108 additions & 0 deletions matlab/kf_linear.m
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,108 @@
% Kalman Filter demonstration with sine signal.

%
% Create sine function
%
sd = 0.1;
dt = 0.1;
w = 1;
T = (0:dt:30);
X = [2*w*T;3*w*T];
Y = X + sd*randn(2,size(X,2));

%
% Initialize KF to values
%
% x = 0
% dx/dt = 0
%
% with great uncertainty in derivative
%
M = [0;0;0;0];
P = diag([0.1 2 0.5 0.5]);
R = sd^2;

% Measurement model.
H = [1 0 0 0;
0 1 0 0];

% Process noise variance
q = 0.1;

% Transition matrix for the continous-time system.
F = [0 0 1 0;
0 0 0 1;
0 0 0 0;
0 0 0 0];
L = [0 0;
0 0;
1 0;
0 1];
[A,Q] = lti_disc(F,L,diag([q q]),dt);

%
% Track and animate
%
MM = zeros(size(M,1),size(Y,2));
PP = zeros(size(M,1),size(M,1),size(Y,2));
clf;
clc;
disp('In this demonstration we estimate a stationary sine signal from noisy measurements by using the classical Kalman filter.');
disp(' ');
disp('The filtering results are now displayed sequantially for 10 time step at a time.');
disp(' ');
disp('<push any key to proceed to next time steps>');

for k=1:size(Y,2)
%
% Track with KF
%
[M,P] = kf_predict(M,P,A,Q);
[M,P] = kf_update(M,P,Y(k),H,R);

MM(:,k) = M;
PP(:,:,k) = P;

%
% Animate
%
if rem(k,10)==1
plot(X(1,:),X(2,:),'b--',...
Y(1,:),Y(2,:),'ro',...
M(1,1),M(2,1),'k*',...
MM(1,1:k),MM(2,1:k),'k-');
legend('Real signal','Measurements','Latest estimate','Filtered estimate')
title('Estimating a noisy sine signal with Kalman filter.');
drawnow;

pause;
end
end

clc;
disp('In this demonstration we estimate a stationary sine signal from noisy measurements by using the classical Kalman filter.');
disp(' ');
disp('The filtering results are now displayed sequantially for 10 time step at a time.');
disp(' ');
disp('<push any key to see the filtered and smoothed results together>')
pause;
%
% Apply Kalman smoother
%
SM = rts_smooth(MM,PP,A,Q);
plot(X(1,:),X(2,:),'b--',...
MM(1,:),MM(2,:),'k-',...
SM(1,:),SM(2,:),'r-');
legend('Real signal','Filtered estimate','Smoothed estimate')
title('Filtered and smoothed estimate of the original signal');

% clc;
% disp('The filtered and smoothed estimates of the signal are now displayed.')
% disp(' ');
% disp('RMS errors:');
%
% Errors
%
% fprintf('KF = %.3f\nRTS = %.3f\n',...
% sqrt(mean((MM(1,:)-X(1,:)).^2)),...
% sqrt(mean((SM(1,:)-X(1,:)).^2)));
99 changes: 99 additions & 0 deletions matlab/radar_estimator_ls/radar_estimator_batch.m
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,99 @@
% Radar Estimator - Batch Estimator
% Author:qingyuanxingsi
% 2015-07-22

% time step
T = 1;

% duration
duration = 60;

% Radar Parameter Configuration
% System Deviation and noise
radar_deviation = [20 8 15;
0.03 0.04 0.01;
0.02 0.02 0.02;
0.01 0.01 0.01;
0.01 0.01 0.01;
0.01 0.01 0.01];

% Radar position
radar_pos = [50 70 20;
120 150 160;
20 30 30];


% target initial states
target_init_state = [100 60 200;
100 80 100;
10 20 30;
10 15 5;
20 10 10;
10 5 10;
0 0 0;
0 0 0;
0 0 0];

% target transition matrix
F = [1 0 0 T 0 0 0 0 0;
0 1 0 0 T 0 0 0 0;
0 0 1 0 0 T 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 0 0;
0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0];

radar_cnt = size(radar_pos,2);
iters = duration/T;
target_num = size(target_init_state,2);
history = zeros(radar_cnt,target_num,3,iters);
measure_history = zeros(radar_cnt,target_num,3,iters);
for tag=1:radar_cnt
target_pos = target_init_state;
radar_position = radar_pos(:,tag);
Y = [];
A = [];
for index=1:iters
target_pos = F*target_pos;
for i=1:target_num
target = target_pos(1:3,i);
history(tag,i,1:3,index) = target;
dis = sqrt(sum((radar_position-target).^2));
angle = atan((radar_position(2)-target(2))/(radar_position(1)-target(1)));
tmp_dis = sqrt(sum((radar_position(1:2) - target(1:2)).^2));
pitch_angle = atan((target(3)-radar_position(3))/tmp_dis);
m_dist = dis+radar_deviation(1,tag)+randn*radar_deviation(4,tag);
m_angle = angle + radar_deviation(2,tag)+randn*radar_deviation(5,tag);
m_pitch_angle = pitch_angle + radar_deviation(3,tag)+randn*radar_deviation(6,tag);
measure_position = [m_dist*cos(m_pitch_angle)*cos(m_angle)+radar_position(1);
m_dist*cos(m_pitch_angle)*sin(m_angle)+radar_position(2);
m_dist*sin(m_pitch_angle)+radar_position(3)];
measure_history(tag,i,1:3,index) = measure_position;
y = measure_position - target;
Y = [Y;y];
a = [cos(pitch_angle)*cos(angle) -dis*cos(pitch_angle)*sin(angle) -dis*sin(pitch_angle)*cos(angle);
cos(pitch_angle)*sin(angle) dis*cos(pitch_angle)*cos(angle) -dis*sin(pitch_angle)*sin(angle);
sin(pitch_angle) 0 dis*cos(pitch_angle)];
A = [A;a];
end
end
estimate_error = inv(A'*A)*A'*Y
end

% Plot the trajectory of the targets measured by different sensors
close all;
for tag=1:radar_cnt
for i=1:target_num
path = reshape(history(tag,i,:,:),3,iters);
measure_path = reshape(measure_history(tag,i,:,:),3,iters);
plot_index = (tag-1)*target_num+i;
subplot(radar_cnt,target_num,plot_index);
plot3(path(1,:),path(2,:),path(3,:))
grid on
hold on
plot3(measure_path(1,:),measure_path(2,:),measure_path(3,:))
title(['Radar ' num2str(tag) ' Target ' num2str(i)])
end
end
96 changes: 96 additions & 0 deletions matlab/radar_estimator_ls/radar_estimator_recursive.m
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,96 @@
% Radar Estimator - Recursive Estimator
% Author:qingyuanxingsi
% 2015-07-22

% time step
T = 1;

% duration
duration = 60;

% Radar Parameter Configuration
% System Deviation and noise
radar_deviation = [20 8 15;
0.03 0.04 0.01;
0.02 0.02 0.02;
0.01 0.01 0.01;
0.01 0.01 0.01;
0.01 0.01 0.01];

% Radar position
radar_pos = [50 70 20;
120 150 160;
20 30 30];


% target initial states
target_init_state = [100 60 200;
100 80 100;
0 0 0;
10 15 5;
20 10 10;
0 0 0;
0 0 0;
0 0 0;
0 0 0];

% target transition matrix
F = [1 0 0 T 0 0 0 0 0;
0 1 0 0 T 0 0 0 0;
0 0 1 0 0 T 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 0 0;
0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0];

radar_cnt = size(radar_pos,2);
iters = duration/T;
target_num = size(target_init_state,2);
history = zeros(radar_cnt,target_num,3,iters);
measure_history = zeros(radar_cnt,target_num,3,iters);
for tag=1:radar_cnt
target_pos = target_init_state;
radar_position = radar_pos(:,tag);
Y = [];
A = [];
iterations = radar_cnt*iters;
theta = zeros(3,iterations+1);
P = zeros(3,3,iterations+1);
theta(:,1) = zeros(3,1);
P(:,:,1) = 10^4*eye(3);
cnt = 2;
for index=1:iters
target_pos = F*target_pos;
for i=1:target_num
target = target_pos(1:3,i);
history(tag,i,1:3,index) = target;
dis = sqrt(sum((radar_position-target).^2));
angle = atan((radar_position(2)-target(2))/(radar_position(1)-target(1)));
tmp_dis = sqrt(sum((radar_position(1:2) - target(1:2)).^2));
pitch_angle = atan((target(3)-radar_position(3))/tmp_dis);
m_dist = dis+radar_deviation(1,tag)+randn*radar_deviation(4,tag);
m_angle = angle + radar_deviation(2,tag)+randn*radar_deviation(5,tag);
m_pitch_angle = pitch_angle + radar_deviation(3,tag)+randn*radar_deviation(6,tag);
measure_position = [m_dist*cos(m_pitch_angle)*cos(m_angle)+radar_position(1);
m_dist*cos(m_pitch_angle)*sin(m_angle)+radar_position(2);
m_dist*sin(m_pitch_angle)+radar_position(3)];
measure_history(tag,i,1:3,index) = measure_position;
y = measure_position - target;
a = [cos(pitch_angle)*cos(angle) -dis*cos(pitch_angle)*sin(angle) -dis*sin(pitch_angle)*cos(angle);
cos(pitch_angle)*sin(angle) dis*cos(pitch_angle)*cos(angle) -dis*sin(pitch_angle)*sin(angle);
sin(pitch_angle) 0 dis*cos(pitch_angle)];
% Update the parameters
for j=1:3
phi = a(j,:)';
invPi = inv(P(:,:,cnt-1))+phi*phi';
P(:,:,cnt) = inv(invPi);
K = P(:,:,cnt-1)*phi*inv(1+phi'*P(:,:,cnt-1)*phi);
theta(:,cnt) = theta(:,cnt-1) + K*(y(j)-phi'*theta(:,cnt-1));
cnt = cnt+1;
end
end
end
theta(:,size(theta,2))
end
84 changes: 84 additions & 0 deletions matlab/radar_estimator_ls/radar_estimator_test.asv
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,84 @@
% Radar Estimator - Batch Estimator
% Author:qingyuanxingsi
% 2015-07-22

function estimation_error = radar_estimator_test(T)

estimation_error = [];
% duration
duration = 60;

% Radar Parameter Configuration
% System Deviation and noise
radar_deviation = [20 8;
0.03 0.04;
0.02 0.02;
0.01 0.01;
0.01 0.01;
0.01 0.01];

% Radar position
radar_pos = [50 70;
120 150;
20 30];


% target initial states
target_init_state = [100 60 200;
100 80 100;
10 20 30;
10 15 5;
20 10 10;
10 5 10;
0 0 0;
0 0 0;
0 0 0];

% target transition matrix
F = [1 0 0 T 0 0 0 0 0;
0 1 0 0 T 0 0 0 0;
0 0 1 0 0 T 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 0 0;
0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0];

radar_cnt = size(radar_pos,2);
iters = floor(duration/T);
target_num = size(target_init_state,2);
history = zeros(radar_cnt,target_num,3,iters);
measure_history = zeros(radar_cnt,target_num,3,iters);
for tag=1:radar_cnt
target_pos = target_init_state;
radar_position = radar_pos(:,tag);
Y = [];
A = [];
for index=1:iters
target_pos = F*target_pos;
for i=1:target_num
target = target_pos(1:3,i);
history(tag,i,1:3,index) = target;
dis = sqrt(sum((radar_position-target).^2));
angle = atan((radar_position(2)-target(2))/(radar_position(1)-target(1)));
tmp_dis = sqrt(sum((radar_position(1:2) - target(1:2)).^2));
pitch_angle = atan((target(3)-radar_position(3))/tmp_dis);
m_dist = dis+radar_deviation(1,tag)+randn*radar_deviation(4,tag);
m_angle = angle + radar_deviation(2,tag)+randn*radar_deviation(5,tag);
m_pitch_angle = pitch_angle + radar_deviation(3,tag)+randn*radar_deviation(6,tag);
measure_position = [m_dist*cos(m_pitch_angle)*cos(m_angle)+radar_position(1);
m_dist*cos(m_pitch_angle)*sin(m_angle)+radar_position(2);
m_dist*sin(m_pitch_angle)+radar_position(3)];
measure_history(tag,i,1:3,index) = measure_position;
y = measure_position - target;
Y = [Y;y];
a = [cos(pitch_angle)*cos(angle) -dis*cos(pitch_angle)*sin(angle) -dis*sin(pitch_angle)*cos(angle);
cos(pitch_angle)*sin(angle) dis*cos(pitch_angle)*cos(angle) -dis*sin(pitch_angle)*sin(angle);
sin(pitch_angle) 0 dis*cos(pitch_angle)];
A = [A;a];
end
end
estimation_error = [estimation_error abs(radar_deviation(1:3,tag)-inv(A'*A)*A'*Y)];
end
estimation_error
Loading

0 comments on commit 27dc200

Please sign in to comment.