Applied-Estimation-Final-Project-/Simulation/Sensor.py at main · 03alebassi-debug/Applied-Estimation-Final-Project- · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
"""
Simulates measurements, including outliers
"""

import numpy as np

class Sensor:
    def __init__(self, range, sigma_d, sigma_theta, p_dropout, p_spurious, p_glitch, bias):
        self.range = range
        self.sigma_d = sigma_d
        self.sigma_theta = sigma_theta
        self.p_dropout = p_dropout
        self.p_spurious = p_spurious
        self.p_glitch = p_glitch
        self.bias = bias

    def measure(self, state, landmark_positions):
        measurements = []
        diffs = landmark_positions - state[0:2]
        distances = np.linalg.norm(diffs, axis=1)

        for i in range(len(landmark_positions)):
            if distances[i] < self.range:

                p = np.random.rand()
                if p < self.p_dropout:
                    continue

                p = np.random.rand()
                if p < self.p_spurious:
                    m_dist = np.random.uniform(0, self.range)
                    m_bearing = np.atan2(diffs[i][1], diffs[i][0]) - state[2] + np.random.uniform(-np.pi, np.pi)
                    m_bearing = self.wrap(m_bearing)
                    measurements.append([m_dist, m_bearing])
                    continue

                p = np.random.rand()
                if p < self.p_glitch:
                    m_dist = distances[i] + np.random.normal(self.bias, self.sigma_d)
                    m_bearing = np.atan2(diffs[i][1], diffs[i][0]) - state[2] + np.random.normal(0, self.sigma_theta)
                    m_bearing = self.wrap(m_bearing)
                    measurements.append([m_dist, m_bearing])
                    continue

                m_dist = distances[i] + np.random.normal(0, self.sigma_d)
                m_bearing = np.atan2(diffs[i][1], diffs[i][0]) - state[2] + np.random.normal(0, self.sigma_theta)
                m_bearing = self.wrap(m_bearing)
                measurements.append([m_dist, m_bearing])

        return measurements

    def wrap(self, angle):
        return (angle + np.pi) % (2*np.pi) - np.pi