508.sagews

This worksheet visualizes the error distribution of a system through a nonlinear function.

The particular system is a distance sensor that can be rotated either on one or two axis. Given are the nonlinear transformation functions, the mean (or expected) value of each sensor and its standard deviation.

import numpy;

def mapPolToKart(r,theta):
    #converts Polar to Kartesian Koordinates
    ret = vector(RDF,[0,0]);
    ret[0] = r*sin(theta);
    ret[1] = r*cos(theta);
    
    return ret;

#Expected Value and Standard Deviation of the distance sensor
r = 10;    #[m]
rStdDev = 0.2; #[m]

#Expected Value and Standard Deviation of the angle sensor
theta = 0; theta = theta/180*pi; #[deg]
thetaStdDev = 20; thetaStdDev = thetaStdDev/180*pi; #[deg]

#transform koordinates
#random values with standard deviation of the sensors around their mean are produced and transformed
amountPoints = 10^4;
polar = [(numpy.random.normal(r, rStdDev), numpy.random.normal(theta, thetaStdDev)) for i in range(0, amountPoints, 1)];
kart = [mapPolToKart(polar[i][0], polar[i][1]) for i in range(0, amountPoints, 1)];

def mean(list, ran, dim):
    #The routine calculates the mean value of a 2 or 3 dimensional (dim) vector-list (list), 
    #given the amount of elements (ran).
    
    if(dim == 2):
        ret = vector(RDF,[0,0]);
    elif(dim == 3):
        ret = vector(RDF,[0,0,0]);
    
    for i in range(0, ran, 1):
        ret[0] += list[i][0];
        ret[1] += list[i][1];
        if(dim == 3):
            ret[2] += list[i][2];
        
        
    ret[0] /= ran;
    ret[1] /= ran;
    if(dim == 3):
        ret[2] /= ran;
    
    return ret;

#calculate mean values
meanPol = mean(polar, amountPoints, 2);
meanPol = mapPolToKart(meanPol[0],meanPol[1]); #transform true mean into kartesian koordinates (for plotting)
print meanPol;

meanKart = mean(kart, amountPoints, 2);
print meanKart;

(-0.0680625988562, 9.99952961266) (-0.0656415672108, 9.40342711233)

#plot the distribution
gcir = 0.4; P = circle((0,0), r, rgbcolor=(gcir,gcir,gcir)); #circle of the mean distance
P += plot(points(kart, pointsize = 2, rgbcolor = "red"), marker = '.');

#plot mean values
P += plot(point(meanPol, pointsize = 20, rgbcolor="green")); #true mean calculated from original spherical koordinates
P += text("true Mean", (meanPol[0]+r*0.8,meanPol[1]),fontsize=10,rgbcolor="green");
P += plot(point(meanKart, pointsize = 20, rgbcolor="blue")); #mean value calculated from transformed koordinates
P += text("trans Mean", (meanKart[0]+r*0.8,meanKart[1]),fontsize=10,rgbcolor="blue");

P.set_aspect_ratio(1);
fy = r+rStdDev; 
fx = (r+rStdDev)*2;
P.set_axes_range(-fx/2,fx/2,0,fy);
P.show();

The errors in the angle measurement lets the points lie on a circle. 

The errors in the distance alters the point position from the mean (circle)

def mapSpherTo3dKart(r,theta,phi):
    #converts Sperical to 3d Kartesian Koordinates
    ret = vector(RDF,[0,0,0]);
    ret[0] = r*sin(theta)*cos(phi);
    ret[1] = r*sin(theta)*sin(phi);
    ret[2] = r*cos(theta);
    
    return ret;

#mean value and standard deviation of the distance sensor
rS = 10;    #[m]
rSStdDev = 0.002; #[m]

#mean value and standard deviation of the 1st angle sensor
Stheta = 50; Stheta = Stheta/180*pi; #[deg]
SthetaStdDev = 15; SthetaStdDev = SthetaStdDev/180*pi; #[deg]

#mean value and standard deviation of the 2nd angle sensor
Sphi = 15; Sphi = Sphi/180*pi; #[deg]
SphiStdDev = 15; SphiStdDev = SphiStdDev/180*pi; #[deg]

#transform all points, from spherical to kartesian
#create pseudo measurement with random normal distrubution and 
#the given sensor output +- errors within its standard deviation
SamountOfPoints = 10^3;
spherical = [[numpy.random.normal(rS, rSStdDev), numpy.random.normal(Stheta, SthetaStdDev), numpy.random.normal(Sphi, SphiStdDev)] for i in range(0, SamountOfPoints, 1)];

Kartesian3d = [mapSpherTo3dKart(spherical[i][0],spherical[i][1],spherical[i][2],) for i in range(0, SamountOfPoints, 1)];

#calculate mean values
meanSpherS = mean(spherical, SamountOfPoints, 3);
meanSpher = mapSpherTo3dKart(meanSpherS[0], meanSpherS[1], meanSpherS[2]);
print meanSpher;

meanKart3d = mean(Kartesian3d, SamountOfPoints, 3);
print meanKart3d;

(7.42006319632, 2.03655236428, 6.38702926678) (6.9473343988, 1.90814339484, 6.18658436533)

#draw the distribution of the measurements
SpherPlot = points(Kartesian3d, rgbcolor = "red", size = 5);
SpherPlot.aspect_ratio((1,1,1));

#add sphere with the raius of the mean distance
SpherPlot += sphere((0,0,0), rS, color='black', opacity=0, aspect_ratio=[1,1,1]);

#Add reference axes
SpherPlot += arrow3d([0,0,0],[rS,0,0], color='red');
SpherPlot += arrow3d([0,0,0],[0,rS,0], color='green');
SpherPlot += arrow3d([0,0,0],[0,0,rS], color='blue');
SpherPlot += text3d("X",(rS+1,0,0), color='red') + text3d("Y",(0,rS+1,0), color='green') + text3d("Z",(0,0,rS+1), color='blue');

#Add mean values
SpherPlot += point3d((meanSpher[0], meanSpher[1], meanSpher[2]), size = 7, rgbcolor="green");
SpherPlot += line3d([(0,0,0), mapSpherTo3dKart(meanSpherS[0]*1.2, meanSpherS[1], meanSpherS[2])], rgbcolor="green");
SpherPlot += point3d((meanKart3d[0], meanKart3d[1], meanKart3d[2]), size = 7, rgbcolor="blue");

#Draw arcs to visualize the errors in the angles
#The width of the arcs is 3 times the Standard Deviation

overs = 10; #overshoot of the arcs in [deg]
StdDevFactor = 3;

SpherPlot += line3d([mapSpherTo3dKart(rS, i/180*pi, Sphi+SphiStdDev*StdDevFactor) 
for i in range((Stheta-SthetaStdDev*StdDevFactor)*180/pi-overs,(Stheta+SthetaStdDev*StdDevFactor)*180/pi+overs)]);
SpherPlot += line3d([mapSpherTo3dKart(rS, i/180*pi, Sphi-SphiStdDev*StdDevFactor) 
for i in range((Stheta-SthetaStdDev*StdDevFactor)*180/pi-overs,(Stheta+SthetaStdDev*StdDevFactor)*180/pi+overs)]);

SpherPlot += line3d([mapSpherTo3dKart(rS, Stheta+SthetaStdDev*StdDevFactor, i/180*pi) 
for i in range((Sphi-SphiStdDev*StdDevFactor)*180/pi-overs,(Sphi+SphiStdDev*StdDevFactor)*180/pi+overs)]);
SpherPlot += line3d([mapSpherTo3dKart(rS, Stheta-SthetaStdDev*StdDevFactor, i/180*pi) 
for i in range((Sphi-SphiStdDev*StdDevFactor)*180/pi-overs,(Sphi+SphiStdDev*StdDevFactor)*180/pi+overs)]);


SpherPlot.show();