calcmeascov2.m

一个目标跟踪系统的MATLAB 源程序包

% 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)];