多源传感器融合作业1报告

多源信息融合

课程作业报告

名称：传感器系统偏差估计问题

姓名：徐妍

班级：自动化46

学号：2140504131

摘要

此次作业围绕多源信息融合课程第二章参数估计中的最小二乘估计展开。在量测方程的计算、非线性方程线性化、线性批处理最小二乘估计、线性递推最小二乘估计、迭代非线性最小二乘估计、非线性递推最小二乘估计等方法进行了实践。本作业使用MATLAB软件编程，完成了既定的题目要求。

一、问题重述及分析

（一）计算出每时刻目标的位置，2部雷达各自观测到的目标位置，并画到一张图上

根据量测方程（1），x 、y 的真实位置以及其他量已知的情况下，两部雷达对应r 及θ的观测值可通过分别计算得出。

00()()()()arctan ()()θθθ?=?????-=+?+? ?-????

r r i w i y i y i w i x i x （1） 在作图时则需注意将极坐标系下的量测值通过坐标变化，转换到直角坐标系下对应的值。在MATLAB 中，使用plot 语句作图即可。

（二）分别用以下4种方法估计2部雷达的角度系统偏差

两部雷达的位置分别使用),(11y x 和),(22y x 表示，则对于每一个时刻k ，均存在如下等式

???+?-+=?-++?-+=?-+22,2,221,1,1112,2,221,1,11)sin()sin()cos()cos(ωθθθθωθθθθk k k k

k k k k r y r y r x r x 其中1ω和2ω为两个与其他量相关的误差量，在此题目中忽略，算入总误差中。最终，代入),(11y x 、),(22y x 的值化简上式得到

??????-=?-?-+=?-)

sin()sin()cos(10)cos(2,2,21,1,12,2,241,1,1θθθθθθθθk k k k k k k k r r r r （2） 1、将非线性方程提前线性化后，用批处理最小二乘法求解

公式（2）为一组非线性方程，在使用线性批处理最小二乘法求解时，需要利用其在()00处的泰勒展开式将其线性化。为简化公式的泰勒展开过程，对公式（2）进行调整得到公式（3）

???

?????+≈?-=+-?+≈?-=--k k k k k k k k k k k k r r r r r r ,11,11,1,182,22,1,22,22,2,282,22,1sin cos )cos(2000010sin cos )cos(2000010θθθθθθθθθθ （3） 由此将其化为矩阵形式

??

????????????=??????????????-+----21,1,2,1,182,22,1,2,282,22,10sin sin 0

cos 2000010cos 2000010θθθθθk k k k k k k k k k r r r r r r 由此可推得??????????????-+----=k k k k k k k k r r r r r r k z ,1,182,22,1,2,282,22,1cos 2000010cos 2000010)(θθ，??

????=0sin sin 0)(,1,2k k k H θ，按照线性批处理最小二乘方法对应的公式分别将H 和z 累积并计算即可。

2、将非线性方程提前线性化后，用递推最小二乘法求解

此题的分析方法与上题大致相同，只是在递推开始的前两个时刻需要使用线性批处理最小二乘方法进行估计得到递推初值，然后套用下述公式循环至截止时刻前一时刻截至即可。

???

????+++-=++-+++=+++=+++++=+-'1'')1()1()1()()1()](?)1()1()[1()(?)1(?)1()1()()1()1()1()()1()1(k W k S k W k P k P k x k H k z k W k x k x

k S k H k P k W k R k H k P k H k S 3、非线性迭代批处理最小二乘法

非线性最小二乘方法的求解过程中的线性化方法与前两题类似，区别在于在每次估计中都会在上一时刻得到的估计值处重新进行泰勒展开，由此导致量测和方程的雅可比矩阵值不断变化。

在迭代非线性批处理最小二乘法中,将公式（2）作适当的变形处理并泰勒展开得到下式

???

?????-??-+?+≈+-?-??-+?+≈--)?)(?sin()?cos(2000010)?)(?sin()?cos(2000010111,11,1,182,22,1222,22,2,282,22,1i i k i k k k k i i k i k k k k r r r r r r θθθθθθθθθθθθ 改写成矩阵形式如下

?????????-??-??????????-?-=??????

?????????+-+-?+--i i i k i k i k k k k i k k k k r r r r r r 22111,12,21,1,182,22,12,2,282,22,1??0)?sin()?sin(0)?cos(2000010)?cos(-2000010θθθθθθθθθθθθ

其中，??????

?????????+-+-?+--=)?cos(2000010)?cos(-20000101,1,182,22,12,2,282,22,1i k k k k i k k k k r r r r r r Z θθθθ，?????????-?-=0)?sin()?sin(01,12,2i k i k i J θθθθ。利用下式不断迭代直至前后两次估计值差值低于既定阈值停止即可。

))(()(1'11'1i i i i i i X h Z R J J R J X X -+=---+

4、非线性递推最小二乘法

非线性递推最小二乘法与线性递推方法相似，区别在于其要求每次递推均在新的估计值处重新进行泰勒展开，公式如下：

???

????+++-=+-+++=++=++++=+-'1'')1()1()1()()1())](?()1()[1()(?)1(?)1()()1()1()1()()1(k W k S k W k P k P k x h k z k W k x k x

k S J k P k W k R k H k P J k S k k 二、结果讨论

（一）每时刻目标的位置及两部雷达各自观测到的目标位置

（二）估计2部雷达的角度系统偏差

源代码

clear;clc;close all ;

%% generate data

sigma_theta = 0.05/180*pi;

delta1 = 0.5/180*pi;

delta2 = -0.8/180*pi;

x1 = [0 0]';

x2 = [10000 0]';

x0 = 0; y0 = 10000;

x0_v = 100;

x = zeros(2,100);

x(1,:) = 0:x0_v:99*x0_v;

x(2,:) = repmat(y0,[1,100]);

%% Q1 location of the goal by 2 radars

% for radar a

r1 =

sqrt((x(1,:)-repmat(x1(1),[1,100])).^2+(x(2,:)-repmat(x1(2),[1,100])).^2)+sigma_r*randn(1,100); theta1 = zeros(1,100);

x_a = zeros(1,100);

y_a = zeros(1,100);

for j = 1:100

theta1(1,j) = atan2((x(2,j)-x1(2)),(x(1,j)-x1(1)))+delta1+sigma_theta*randn;

x_a(1,j) = r1(1,j)*cos(theta1(1,j))+x1(1);

y_a(1,j) = r1(1,j)*sin(theta1(1,j))+x1(2);

end

% for radar b

r2 =

sqrt((x(1,:)-repmat(x2(1),[1,100])).^2+(x(2,:)-repmat(x2(2),[1,100])).^2)+sigma_r*randn(1,100); theta2 = zeros(1,100);

x_b = zeros(1,100);

y_b = zeros(1,100);

for j = 1:100

theta2(1,j) = atan2((x(2,j)-x2(2)),(x(1,j)-x2(1)))+delta2+sigma_theta*randn;

x_b(1,j) = r2(1,j)*cos(theta2(1,j))+x2(1);

y_b(1,j) = r2(1,j)*sin(theta2(1,j))+x2(2);

end

% plot

figure(1),

plot(x_a,y_a,'r',x_b,y_b,'b',x(1,:),x(2,:),'k');

legend('radar a','radar b','real track');

%% Q2 estimate systematic error of the angle

% clear delta1;

% for R = I, R can be omitted

%%%%%%%%%%%%%%%% linear-batch

LS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% H_batch_k = [];

z1_batch = (r1.^2-r2.^2-10^8)./20000./r2-cos(theta2);

z2_batch = (r1.^2-r2.^2+10^8)./20000./r1-cos(theta1);

z_batch_k = [];

x_batch = [];

for i = 1:100

H = [0 sin(theta2(i));sin(theta1(i)) 0];

H_batch_k = [H_batch_k;H];

z_batch_k=[z_batch_k;

z1_batch(i);z2_batch(i)];

x_batch(:,i)=inv(H_batch_k'*H_batch_k)*H_batch_k'*z_batch_k;

end

% result of linear batch least squares estimate

x_batch_result = x_batch(:,100)/pi*180;

disp('result of linear batch least squares estimate');

disp(x_batch_result);

%%%%%%%%%%%%%% linear recursive

LS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

H_recursive = [0 sin(theta2(1));sin(theta1(1)) 0;

0 sin(theta2(2));sin(theta1(2)) 0];

z_recursive = [z1_batch(1);z2_batch(1);

z1_batch(2);z2_batch(2)];

x_recursive(:,2) = inv(H_recursive'*H_recursive)*H_recursive'*z_recursive;

P_recursive(:,:,2)=inv(H_recursive'*H_recursive);

R = eye(2);

for i=2:99

H=[0 sin(theta2(i+1));sin(theta1(i+1)) 0];

S=H*P_recursive(:,:,i)*H'+R;

W=P_recursive(:,:,i)*H'*inv(S);

z = [z1_batch(i+1);z2_batch(i+1)];

x_recursive(:,i+1)=x_recursive(:,i)+W*(z-H*x_recursive(:,i));

P_recursive(:,:,i+1)=P_recursive(:,:,i)-W*S*W';

end

% result of linear recursive least squares estimate

x_recursive_result = x_recursive(:,100)/pi*180;

disp('result of linear recursive least squares estimate');

disp(x_recursive_result);

%%%%%%%%%%%%%% nonlinear iterative batch

LS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

x_ILS(:,1) = [0;0];

Dif = 100;

j = 1;

% R is omitted, for R = I

z_batch_iter = [];

z1_batch_iter = (r1.^2-r2.^2-10^8)./20000./r2;

z2_batch_iter = (r1.^2-r2.^2+10^8)./20000./r1;

for i = 2:100

z_batch_iter = [z_batch_iter;

z1_batch_iter(i);z2_batch_iter(i)];

end

while Dif>10^(-20)

% while j<100

J = [];hx = [];

for i = 2:100

JJ = [0 sin(theta2(i)-x_ILS(2,j));

sin(theta1(i)-x_ILS(1,j)) 0];

J = [J

JJ];

hx = [hx

cos(theta2(i)-x_ILS(2,j));

cos(theta1(i)-x_ILS(1,j))];

end

x_ILS(:,j+1) = x_ILS(:,j)+inv(J'*J)*J'*(z_batch_iter-hx);

Dif = sum((x_ILS(:,j+1)-x_ILS(:,j)).^2);

j = j+1;

end

% result of nonlinear iterative batch least squares estimate

x_iterative_result = x_ILS(:,j-1)/pi*180;

disp('result of nonlinear iterative batch least squares estimate');

disp(x_iterative_result);

%%%%%%%%%%%%%% nonlinear recursive

LS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear hx;

R_RLS = eye(2);

x_RLS(:,1) = [0;0];

P(:,:,1) = diag([10^4 10^4]);

for i = 2:100

H = [0 sin(theta2(i)-x_RLS(2,i-1));

sin(theta1(i)-x_RLS(1,i-1)) 0];

S=H*P(:,:,i-1)*H'+R_RLS;

W=P(:,:,i-1)*H'*inv(S);

Z_RLS(:,i)=[z1_batch_iter(i);z2_batch_iter(i)];

hx = [cos(theta2(i)-x_RLS(2,i-1));

cos(theta1(i)-x_RLS(1,i-1))];

x_RLS(:,i)=x_RLS(:,i-1)+W*(Z_RLS(:,i)-hx);

P(:,:,i)=P(:,:,i-1)-W*S*W';

end

% result of nonlinear recursive least squares estimate

x_nonlinear_recursive_result = x_RLS(:,i)/pi*180;

disp('result of nonlinear recursive least squares estimate');

disp(x_nonlinear_recursive_result);

% plot the result of case 3/4

figure;

t = 1:100;

plot(t,repmat(0.5,1,100),'k',t,x_recursive(1,:)./pi.*180,'g',t,x_RLS(1,:)./pi.*180,'r');

axis([1,100,0,0.7]);

legend('真值','线性递推估计值','非线性递推估计值');

title('雷达1系统偏差delta1');

figure;

plot(t,repmat(-0.8,1,100),'k',t,x_recursive(2,:)./pi.*180,'g',t,x_RLS(2,:)./pi.*180,'r');

axis([1,100,-1,0]);

legend('真值','线性递推估计值','非线性递推估计值');

title('雷达2系统偏差delta2');

figure;

t = 1:size(x_ILS,2);

plot(t,repmat(0.5,1,length(t)),'k',t,repmat(-0.8,1,length(t)),'b',t,x_ILS(1,:)./pi.*180,'g',t,x_ILS(2,:)./ pi.*180,'r');

axis([1,length(t),-1,1]);

legend('delta1真值','delta2真值','非线性迭代批处理delta1估计值','非线性迭代批处理delta2估计值');

title('非线性迭代批处理方法系统偏差');