ydlidar_x4_arg_crrection_approx.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Fri Jun  8 10:53:59 2018

@author: akira
"""

"""
from maxima
define(f(d),180/%pi* (atan(21.8*(155.3-d)/(155.3*d)  ) - atan(-21.8/155.3)));
define(t(a,b,x),1/(a*x+b));
define(e(a,b,x),( ( f(x)-t(a,b,x) )*x )**2);

noise power for each distance(mm)
x^2*((180*(atan((0.1403734707018673*(155.3-x))/x)+0.1394622140735531))/%pi-1/(a*x+b))^2


initial value (taylor on dist=1000)
a=
8.162604522317974E-4
b=
-0.00220016337230633
"""


a=8.162604522317974E-4
b=-0.00220016337230633



from scipy.optimize import fmin
import math
import numpy as np

def offset():
    return -180/math.pi* math.atan(-21.8/155.3)

def original(d):
    return 180/math.pi* (math.atan(21.8*(155.3-d)/(155.3*d)))   

def approx(x,a,b):
    return 1/(a*x+b) - offset()
    
def e(a,b,x):
    return x**2*((180*(math.atan((0.1403734707018673*(155.3-x))/x)+0.1394622140735531))/math.pi-1/(a*x+b))**2

def loss(arg):
    a,b=arg
    s=0
    for x in np.linspace(120,10000,1000):
        s+=e(a,b,x)
    return s 

arg=[a,b]
print(loss( arg,))
arg=fmin(loss,arg)

print(loss(arg))
print ("1/(",a,"*x",b,")-",offset())

import matplotlib.pyplot as plt
plt.figure(0)
plt.clf()

X=np.linspace(120,10*1000,1000)
Yorg=np.array([original(x) for x in X])
Yapp=np.array([approx(x,arg[0],arg[1]) for x in X])
plt.plot(X,Yorg-Yapp)
plt.xlabel("distance")
plt.ylabel("Deg")
plt.title("Error of the approx.")
plt.figure(1)
plt.clf()
X=np.linspace(120,10000,1000)
Yorg=np.array([original(x) for x in X])
Yapp=np.array([approx(x,arg[0],arg[1]) for x in X])
plt.xlabel("distance")
plt.ylabel("Deg")
plt.title("Arg correct")
plt.plot(X,Yorg)
plt.plot(X,Yapp)