<SYSTEM_TASK:>
Given the following text description, write Python code to implement the functionality described below step by step
<END_TASK>
<USER_TASK:>
Description:
 This notebook presents the techniques of displaying the precision of the Radio Telemetry Tracker system.
Step1:  Model Estimation
To determine the location of the radio transmitter, we need to find a relationship between the measurement location and the radio amplitude measurement.  We use the radio signal path loss model
Step2:  When we receive data, we can extract the following information
Step3:  To localize the transmitter, we simply take the measurements in $D$ and fit them to the model in receivePowerModel
Step4:  As shown above, the estimated result is nearly identical to the simulated parameters, which should be the case, as the only source of noise here is from quantization error.
In order to accurately simulate the data from the drone, we need to add some error to the measurements.  In this case, we will add Gaussian noise to each measurement.  For the $x$ and $y$ coordinates, we will add Gaussian noise with $\mu = 0$ and $\sigma^2 = 3$.  For the $z$ coordinate, we will add Gaussian noise with $\mu = 0$ and $\sigma^2 = 0.5$.  For the $R$ measurement, we will add Gaussian noise with $\mu = 0$ and $\sigma^2 = 5$.
Step5:  Precision Visualization
In order to provide a metric of how certain our estimate is, we need to provide a way to visualize the precision of our estimate.
Naive Visualization
The easiest way to do this is to assume a distribution over the distance from each ping to the transmitter.  We assume that the distance calculated for each ping has additive Gaussian noise with a standard deviation of 40% of the calculated range.
Because the location of the transmitter is calculated from multiple independent measurements, the probability of the transmitter being at a particular location is equal to the product of the probability of each distance calculation.  We can then use log probability to simplify the calculations.
For a single ping, we demonstrate the probability heatmap below.  This is a simulated ping at $(0, 0)$, with a calculated distance of 50 meters.  The calculated distance is represented by the red circle, and the ping location is represented by the red triangle.
Step6:  For multiple pings, we simply sum the log probability together.  The below code simulates three pings, one at $(-50, 0)$, one at $(0, -50)$, and one at $(35, 35)$.  Each has a calculated distance of 50 meters, with the distance again represented by the red circles, and ping locations represented by the red triangles.
Step7:  In order to generate a heatmap for a dataset, we need to first determine the appropriate heatmap area.  As a prerequisite, we must know the estimated transmitter location and ping locations.  Because the transmitter location might not be within the polygon that encloses all the ping locations, it is essential to take the transmitter location into account when generating the heatmap area.  A simple way to do this would be to take the maximum extents of both estimated transmitter and ping locations.  We can then use the same technique as above to generate the heatmap.
In order to generate the distances from the data, we need to invert the receive power model, which becomes $d = 10^\frac{k - R}{10n}$
<ASSISTANT_TASK:>
Python Code:
import numpy as np
import matplotlib.pyplot as plt; plt.ion()
from scipy.optimize import least_squares
from scipy import stats as st
Explanation: This notebook presents the techniques of displaying the precision of the Radio Telemetry Tracker system.
End of explanation
def receivePowerModel(d, k, n):
    return k - 10 * n * np.log10(d)
Explanation: Model Estimation
To determine the location of the radio transmitter, we need to find a relationship between the measurement location and the radio amplitude measurement.  We use the radio signal path loss model: $L = 10nlog_{10}(d)$.
We can then rewrite this in terms of receive and transmit power, $R$ and $P$: $R = P - 10nlog_{10}(d) + C$, where $C$ represents receive losses not due to path loss.
In theory, $n$ is 2 for path loss in a vacuum, but in practice, we see $n$ range from $2$ to $6$.
However, we cannot in principle differentiate between P and C, as they are summed together, so we can simply combine them into a single parameter $k$: $R = k - 10nlog_{10}(d)$
End of explanation
vx = np.array([[5, 1, 0],
               [-1, 5, 0],
               [-5, -1, 0],
               [1, -5, 0]])
tx = (477954, 3638577, 11, 'S')
txp = 100
n = 4
C = 1
d = 1000
tx_loc = np.array([tx[0], tx[1], 0])
dx_loc = np.array([tx[0] - 100, tx[1], 30])
s_per_leg = 30
c_loc = dx_loc
D_p = []
for leg in range(4):
    for s in range(s_per_leg):
        c_loc += vx[leg,:]
        dist = np.linalg.norm(tx_loc - c_loc)
        R = receivePowerModel(dist, txp - C, n)
        D_p.append([c_loc[0], c_loc[1], c_loc[2], R])
simulated_D = np.array(D_p)
plt.scatter(simulated_D[:,0], simulated_D[:,1], c = simulated_D[:,3])
plt.colorbar(label="Ping Power")
plt.xlabel("Easting (m)")
plt.ylabel("Northing (m)")
plt.title("Simulated Data with No Noise")
plt.show()
Explanation: When we receive data, we can extract the following information: drone location in x, y, and z, and transmitter receive power.  We can then put this information into a matrix $D = \begin{bmatrix}x_d & y_d & z_d & R\end{bmatrix}$, where $x_d$ is the column vector of drone x coordinates, $y_d$ is the column vector of drone y coordinates, $z_d$ is the column vector of drone z coordinates, and $R$ is the column vector of receive powers.  We generate a simulated dataset below:
End of explanation
def residuals(params, data):
    tx = params[0]
    ty = params[1]
    t_loc = np.array([tx, ty, 0])
    
    k = params[2]
    n = params[3]
    
    R = data[:,3]
    d_loc = data[:,0:3]
    
    residuals = np.zeros(len(R))
    for i in range(len(R)):
        residuals[i] = R[i] - receivePowerModel(np.linalg.norm(t_loc - d_loc[i,:]), k, n)
    return residuals
initialGuess = np.array([np.mean(simulated_D[:,0]), np.mean(simulated_D[:,1]),
                         np.max(simulated_D[:,3]), 4])
results = least_squares(residuals, initialGuess, kwargs={'data':simulated_D})
print(results.x)
print("Lateral error: %f" % np.linalg.norm(tx_loc - np.array([results.x[0], results.x[1], 0])))
print("k error: %f" % (txp - C - results.x[2]))
print("n error: %f" % (n - results.x[3]))
Explanation: To localize the transmitter, we simply take the measurements in $D$ and fit them to the model in receivePowerModel
End of explanation
c_loc = dx_loc
D_p = []
s_xy = np.sqrt(3)
s_z = np.sqrt(0.5)
s_R = np.sqrt(5)
for leg in range(4):
    for s in range(s_per_leg):
        c_loc += vx[leg,:]
        dist = np.linalg.norm(tx_loc - c_loc)
        R = receivePowerModel(dist, txp - C, n)
        D_p.append([c_loc[0] + np.random.normal(0,s_xy),
                    c_loc[1] + np.random.normal(0,s_xy),
                    c_loc[2] + np.random.normal(0,s_z),
                    R + np.random.normal(0,s_R)])
simulated_D_error = np.array(D_p)
plt.scatter(simulated_D_error[:,0], simulated_D_error[:,1], c = simulated_D_error[:,3])
plt.colorbar(label="Ping Power")
plt.xlabel("Easting (m)")
plt.ylabel("Northing (m)")
plt.title("Simulated Data with Noise")
plt.show()
initialGuess = np.array([np.mean(simulated_D_error[:,0]), np.mean(simulated_D_error[:,1]),
                         np.max(simulated_D_error[:,3]), 4])
results = least_squares(residuals, initialGuess, kwargs={'data':simulated_D_error})
print(results.x)
print("Lateral error: %f" % np.linalg.norm(tx_loc - np.array([results.x[0], results.x[1], 0])))
print("k error: %f" % (txp - C - results.x[2]))
print("n error: %f" % (n - results.x[3]))
Explanation: As shown above, the estimated result is nearly identical to the simulated parameters, which should be the case, as the only source of noise here is from quantization error.
In order to accurately simulate the data from the drone, we need to add some error to the measurements.  In this case, we will add Gaussian noise to each measurement.  For the $x$ and $y$ coordinates, we will add Gaussian noise with $\mu = 0$ and $\sigma^2 = 3$.  For the $z$ coordinate, we will add Gaussian noise with $\mu = 0$ and $\sigma^2 = 0.5$.  For the $R$ measurement, we will add Gaussian noise with $\mu = 0$ and $\sigma^2 = 5$.
End of explanation
width = 100
dist = 50
field = np.zeros((2 * width, 2 * width))
origin = np.array([width, width])
stddev = 0.4 * dist
mean = dist
rv = st.norm(loc = mean, scale = stddev)
for x in range(field.shape[0]):
    for y in range(field.shape[1]):
        r = np.linalg.norm(np.array([x, y]) - origin)
        field[x, y] = rv.pdf(r)
fig = plt.figure()
plt.imshow(field, extent=(-width, width, -width, width), origin='lower')
plt.colorbar()
plt.scatter(0, 0, marker='^', color='red')
cir = plt.Circle((0, 0), radius=dist, fill=False, edgecolor='red')
fig.gca().add_artist(cir)
plt.xlabel('Easting (m)')
plt.ylabel('Northing (m)')
plt.title('Certainty Distribution for a Single Ping')
plt.show()
Explanation: Precision Visualization
In order to provide a metric of how certain our estimate is, we need to provide a way to visualize the precision of our estimate.
Naive Visualization
The easiest way to do this is to assume a distribution over the distance from each ping to the transmitter.  We assume that the distance calculated for each ping has additive Gaussian noise with a standard deviation of 40% of the calculated range.
Because the location of the transmitter is calculated from multiple independent measurements, the probability of the transmitter being at a particular location is equal to the product of the probability of each distance calculation.  We can then use log probability to simplify the calculations.
For a single ping, we demonstrate the probability heatmap below.  This is a simulated ping at $(0, 0)$, with a calculated distance of 50 meters.  The calculated distance is represented by the red circle, and the ping location is represented by the red triangle.
End of explanation
width = 120
dist = 50
pings = [np.array([-dist, 0]), np.array([0, -dist]), np.array([.7*dist, .7*dist])]
field = np.zeros((2 * width, 2 * width))
origin = np.array([width, width])
stddev = 0.4 * dist
mean = dist
rv = st.norm(loc = mean, scale = stddev)
for x in range(field.shape[0]):
    for y in range(field.shape[1]):
        for ping in pings:
            r = np.linalg.norm(np.array([x, y]) - origin - ping)
            field[x, y] += np.log10(rv.pdf(r))
fig1 = plt.figure()
plt.imshow(np.power(10, field), extent=(-width, width, -width, width), origin='lower')
plt.colorbar()
for ping in pings:
    plt.scatter(ping[0], ping[1], marker='^', color='red')
    cir = plt.Circle(ping, radius=dist, fill=False, edgecolor='red')
    fig1.gca().add_artist(cir)
plt.xlabel('Easting (m)')
plt.ylabel('Northing (m)')
plt.title('Certainty Distribution for a Three Pings')
plt.show()
Explanation: For multiple pings, we simply sum the log probability together.  The below code simulates three pings, one at $(-50, 0)$, one at $(0, -50)$, and one at $(35, 35)$.  Each has a calculated distance of 50 meters, with the distance again represented by the red circles, and ping locations represented by the red triangles.
End of explanation
def distancePowerModel(R, k, n):
    return np.power(10, (k - R) / (10 * n))
simulated_D_error
new_D = simulated_D_error[simulated_D_error[:,3] > 20,:]
tx_loc = results.x[0:2]
n = results.x[3]
k = results.x[2]
resolution = 0.25 #px per meter
border = 20 # meter
max_x = max(np.max(simulated_D_error[:,0]), tx_loc[0]) + border
min_x = min(np.min(simulated_D_error[:,0]), tx_loc[0]) - border
max_y = max(np.max(simulated_D_error[:,1]), tx_loc[1]) + border
min_y = min(np.min(simulated_D_error[:,1]), tx_loc[1]) - border
# origin is top left, 0.5 px / m resolution
field = np.zeros((int((max_x - min_x) * resolution), int((max_y - min_y) * resolution)))
origin = np.array([max_x, min_y])
means = [distancePowerModel(R, k, n) for R in new_D[:,3]]
rv = [st.norm(loc = d, scale = 0.4 * d) for d in means]
for x in range(field.shape[0]):
    for y in range(field.shape[1]):
        px_loc = origin + np.array([-x, y]) / resolution
        for ping_idx in range(len(rv)):
            distance = np.linalg.norm(px_loc - new_D[ping_idx,0:2])
            if distance < 2 * means[ping_idx]:
                field[x, y] += np.log10(rv[ping_idx].pdf(distance))
plt.imshow(np.power(10, field), extent=(min_x, max_x, min_y, max_y), origin="lower")
tx_loc
tx_loc
Explanation: In order to generate a heatmap for a dataset, we need to first determine the appropriate heatmap area.  As a prerequisite, we must know the estimated transmitter location and ping locations.  Because the transmitter location might not be within the polygon that encloses all the ping locations, it is essential to take the transmitter location into account when generating the heatmap area.  A simple way to do this would be to take the maximum extents of both estimated transmitter and ping locations.  We can then use the same technique as above to generate the heatmap.
In order to generate the distances from the data, we need to invert the receive power model, which becomes $d = 10^\frac{k - R}{10n}$
End of explanation
<END_TASK>