www.pudn.com > trackingdemos.zip > CalcMeasCov2.m

% function [x, y, R] = CalcMeasCov2(r, theta, x1, y1, sigma_r, sigma_theta) 
% 
% Convert polar to Cartesian measurements with Covariance Matrix  
% 
% r   ----- measured range 
% theta --- measured bearing 
% x1  ----- sensor's x-position 
% y1  ----- sensor's y-position 
% sigma_r   ---- standard deviation of range measurement 
% sigma_theta -- standard deviation of bearing measurement 
% x   ----- measured target's x-position 
% y   ----- measured target's y-position 
% R   ----- covariance of the measurement vector [x; y] 
function [x, y, R] = CalcMeasCov2(r, theta, x1, y1, sigma_r, sigma_theta) 
R=zeros(2,2); 
sigma_rsq=sigma_r^2; 
sigma_thetasq=sigma_theta^2; 
 
x = r*cos(theta) + x1; 
y = r*sin(theta) + y1; 
 
b=(sigma_rsq + r^2*sigma_thetasq)/(sigma_rsq - r^2*sigma_thetasq); 
const=(sigma_rsq - r^2*sigma_thetasq)/2; 
R = const*[b + cos(2*theta), sin(2*theta); 
	   sin(2*theta), b - cos(2*theta)];