Indoor Navigation Systems for Drones
GPS doesn't work indoors. Drones need alternative positioning systems to navigate inside buildings, warehouses, and caves. Think of it as the drone's ability to find its way through a house without any windows.
Indoor Positioning Challenge
Sponsored
Indoor environments present unique challenges: no GPS, complex multipath for radio signals, dynamic obstacles, and limited space.
Indoor Positioning Systems Comparison
Visual-Inertial Odometry (VIO)
VIO combines camera images with IMU data to estimate motion without external references.
python
from typing import Tuple, Optional
class VisualInertialOdometry:
def __init__(self, camera_matrix, dist_coeffs):
self.camera_matrix = camera_matrix
self.dist_coeffs = dist_coeffs
# State: [position, velocity, orientation, gyro_bias, accel_bias]
self.state = np.zeros(15)
self.covariance = np.eye(15) * 0.01
# Feature tracking
self.orb = cv2.ORB_create(nfeatures=500)
self.prev_frame = None
self.prev_kp = None
self.prev_desc = None
# IMU parameters
self.gravity = np.array([0, 0, 9.81])
self.gyro_noise = 0.01
self.accel_noise = 0.1
def process_frame(self, image: np.ndarray) -> Optional[np.ndarray]:
"""Process camera frame for VIO."""
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
keypoints, descriptors = self.orb.detectAndCompute(gray, None)
if self.prev_frame is not None and self.prev_desc is not None:
# Feature matching
matches = self._match_features(self.prev_desc, descriptors)
if len(matches) > 10:
# Compute relative pose
R, t, inlier_mask = self._estimate_pose(
self.prev_kp, keypoints, matches
)
if R is not None:
# Update state
self._update_state(R, t)
self.prev_frame = gray
self.prev_kp = keypoints
self.prev_desc = descriptors
return self.state[:3].copy()
self.prev_frame = gray
self.prev_kp = keypoints
self.prev_desc = descriptors
return None
def process_imu(self, accel: np.ndarray, gyro: np.ndarray, dt: float):
"""Process IMU measurement for prediction."""
# Remove bias
accel_corrected = accel - self.state[12:15]
# Rotate to world frame
R = self._rotation_from_euler(self.state[6:9])
accel_world = R @ accel_corrected - self.gravity
# Integrate
self.state[3:6] += accel_world * dt
self.state[0:3] += self.state[3:6] * dt + 0.5 * accel_world * dt**2
self.state[6:9] += gyro * dt
# Update covariance
F = self._jacobian_motion(dt)
Q = self._process_noise(dt)
self.covariance = F @ self.covariance @ F.T + Q
def _match_features(self, desc1, desc2):
bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
matches = bf.match(desc1, desc2)
return sorted(matches, key=lambda x: x.distance)[:50]
def _estimate_pose(self, kp1, kp2, matches):
pts1 = np.float32([kp1[m.queryIdx].pt for m in matches])
pts2 = np.float32([kp2[m.trainIdx].pt for m in matches])
E, mask = cv2.findEssentialMat(pts2, pts1, self.camera_matrix,
method=cv2.RANSAC, threshold=1.0)
_, R, t, mask = cv2.recoverPose(E, pts2, pts1, self.camera_matrix)
return R, t, mask
def _update_state(self, R, t):
self.state[0:3] += R @ t.flatten() * 0.1 # Scale factor
def _rotation_from_euler(self, euler):
roll, pitch, yaw = euler
Rx = np.array([[1, 0, 0], [0, np.cos(roll), -np.sin(roll)],
[0, np.sin(roll), np.cos(roll)]])
Ry = np.array([[np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0],
[-np.sin(pitch), 0, np.cos(pitch)]])
Rz = np.array([[np.cos(yaw), -np.sin(yaw), 0],
[np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]])
return Rz @ Ry @ Rx
def _jacobian_motion(self, dt):
F = np.eye(15)
F[0:3, 3:6] = np.eye(3) * dt
return F
def _process_noise(self, dt):
Q = np.zeros((15, 15))
Q[0:3, 0:3] = np.eye(3) * 0.01 * dt**2
Q[3:6, 3:6] = np.eye(3) * self.accel_noise**2 * dt**2
Q[6:9, 6:9] = np.eye(3) * self.gyro_noise**2 * dt**2
Q[12:15, 12:15] = np.eye(3) * 0.001 * dt
return Q
UWB Positioning
Ultra-Wideband (UWB) provides high-accuracy indoor positioning using time-of-flight measurements between anchors and tags.
python
from typing import List, Tuple
from scipy.optimize import least_squares
class UWBPositioning:
def __init__(self, anchors: List[np.ndarray]):
self.anchors = np.array(anchors)
self.n_anchors = len(anchors)
self.tag_height = 1.0 # Height of UWB tag on drone
def trilaterate(self, distances: np.ndarray) -> np.ndarray:
"""Compute position from distance measurements."""
if self.n_anchors < 4:
raise ValueError("Need at least 4 anchors for 3D positioning")
# Initial guess (center of anchors)
x0 = np.mean(self.anchors, axis=0)
# Solve using least squares
result = least_squares(
self._distance_residuals,
x0,
args=(self.anchors, distances),
method='lm'
)
return result.x
def _distance_residuals(self, position, anchors, measured_distances):
"""Residual function for trilateration."""
residuals = []
for i, anchor in enumerate(anchors):
computed_distance = np.linalg.norm(position - anchor)
residuals.append(computed_distance - measured_distances[i])
return np.array(residuals)
def kalman_filter_position(self, measurements: List[np.ndarray],
dt: float = 0.1) -> np.ndarray:
"""Apply Kalman filter for smooth position estimates."""
# State: [x, y, z, vx, vy, vz]
state = np.zeros(6)
P = np.eye(6) * 1.0
F = np.eye(6)
F[0:3, 3:6] = np.eye(3) * dt
H = np.zeros((3, 6))
H[0:3, 0:3] = np.eye(3)
R = np.eye(3) * 0.1 # Measurement noise
Q = np.eye(6) * 0.01 # Process noise
positions = []
for z in measurements:
# Predict
state = F @ state
P = F @ P @ F.T + Q
# Update
y = z - H @ state
S = H @ P @ H.T + R
K = P @ H.T @ np.linalg.inv(S)
state = state + K @ y
P = (np.eye(6) - K @ H) @ P
positions.append(state[:3].copy())
return np.array(positions)
def detect_multipath(self, distances: np.ndarray,
expected_positions: np.ndarray) -> np.ndarray:
"""Detect and mitigate multipath effects."""
clean_distances = distances.copy()
for i, (dist, anchor) in enumerate(zip(distances, self.anchors)):
# Compute expected distance
if expected_positions is not None:
expected_dist = np.linalg.norm(expected_positions - anchor)
# If measurement differs significantly, likely multipath
if abs(dist - expected_dist) > 2.0:
clean_distances[i] = expected_dist
return clean_distances
class UWBAnchorPlacement:
"""Optimize anchor placement for best coverage."""
@staticmethod
def optimize_placement(room_dimensions: Tuple[float, float, float],
n_anchors: int = 4) -> List[np.ndarray]:
"""Compute optimal anchor positions."""
x, y, z = room_dimensions
if n_anchors == 4:
# Tetrahedral placement
return [
np.array([0, 0, z]),
np.array([x, 0, z]),
np.array([0, y, z]),
np.array([x, y, z])
]
elif n_anchors == 8:
# Corner placement
anchors = []
for ix in [0, x]:
for iy in [0, y]:
for iz in [z * 0.8, z]:
anchors.append(np.array([ix, iy, iz]))
return anchors
else:
# Default: distribute evenly
anchors = []
for i in range(n_anchors):
angle = 2 * np.pi * i / n_anchors
anchors.append(np.array([
x/2 + x/3 * np.cos(angle),
y/2 + y/3 * np.sin(angle),
z
]))
return anchors
WiFi-Based Positioning
Sponsored
WiFi received signal strength (RSS) can be used for coarse indoor positioning.
python
from typing import Dict, List, Tuple
from sklearn.neighbors import KNeighborsRegressor
class WiFiPositioning:
def __init__(self):
self.fingerprint_database = {}
self.knn_model = None
def build_fingerprint_map(self, measurements: List[Dict]):
"""Build WiFi fingerprint database."""
for measurement in measurements:
position = measurement['position']
rssi_values = measurement['rssi']
key = tuple(position)
self.fingerprint_database[key] = rssi_values
def train_localization_model(self):
"""Train KNN model for position estimation."""
positions = []
rssi_values = []
for pos, rssi in self.fingerprint_database.items():
positions.append(pos)
rssi_values.append(list(rssi.values()))
self.knn_model = KNeighborsRegressor(n_neighbors=5, weights='distance')
self.knn_model.fit(rssi_values, positions)
def localize(self, current_rssi: Dict) -> np.ndarray:
"""Estimate position from current RSSI measurements."""
if self.knn_model is None:
raise ValueError("Model not trained. Call train_localization_model first.")
rssi_vector = list(current_rssi.values())
position = self.knn_model.predict([rssi_vector])[0]
return np.array(position)
def rssi_to_distance(self, rssi: float, rssi_0: float = -50,
path_loss_exponent: float = 2.5) -> float:
"""Convert RSSI to estimated distance."""
if rssi == 0:
return 0
distance = 10 ** ((rssi_0 - rssi) / (10 * path_loss_exponent))
return distance
def triangulate_from_ap(self, distances: Dict[str, float],
ap_positions: Dict[str, np.ndarray]) -> np.ndarray:
"""Triangulate position from multiple AP distances."""
positions = []
weights = []
for ap_id, distance in distances.items():
if ap_id in ap_positions:
positions.append(ap_positions[ap_id])
# Weight by inverse of estimated distance error
weights.append(1.0 / (distance + 0.1))
if len(positions) < 3:
return None
positions = np.array(positions)
weights = np.array(weights)
weights = weights / weights.sum()
# Weighted average as initial estimate
position = np.average(positions, axis=0, weights=weights)
return position
class WiFiScanManager:
"""Manage WiFi scanning and data collection."""
def __init__(self):
self.scan_history = []
self.current_scan = {}
def perform_scan(self) -> Dict[str, float]:
"""Perform WiFi scan (simulated)."""
# In real implementation, use system WiFi scanning
import random
scan = {}
n_aps = random.randint(3, 8)
for i in range(n_aps):
ap_id = f"AP_{i}"
rssi = random.randint(-80, -30)
scan[ap_id] = rssi
self.current_scan = scan
self.scan_history.append(scan)
return scan
def get_scan_history(self, n_scans: int = 10) -> List[Dict]:
return self.scan_history[-n_scans:]
Visual Landmarks
Pre-placed visual markers (like AprilTags) provide absolute positioning references.
python
from typing import List, Dict, Optional
class VisualLandmarkSystem:
def __init__(self, camera_matrix, dist_coeffs):
self.camera_matrix = camera_matrix
self.dist_coeffs = dist_coeffs
# AprilTag detector
self.detector = cv2.aruco.ArucoDetector(
cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
)
self.landmark_database = {} # id -> (position, size)
self.detected_landmarks = []
def register_landmark(self, landmark_id: int, position: np.ndarray,
size: float = 0.1):
"""Register a known landmark position."""
self.landmark_database[landmark_id] = {
'position': position,
'size': size
}
def detect_landmarks(self, image: np.ndarray) -> List[Dict]:
"""Detect visual landmarks in image."""
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
corners, ids, rejected = self.detector.detectMarkers(gray)
detected = []
if ids is not None:
for i, marker_id in enumerate(ids.flatten()):
if marker_id in self.landmark_database:
# Estimate pose
rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers(
corners[i:i+1],
self.landmark_database[marker_id]['size'],
self.camera_matrix,
self.dist_coeffs
)
detected.append({
'id': marker_id,
'corners': corners[i],
'position': tvec[0][0],
'rotation': rvec[0][0],
'distance': np.linalg.norm(tvec[0][0])
})
self.detected_landmarks = detected
return detected
def compute_position(self, detected: List[Dict]) -> Optional[np.ndarray]:
"""Compute drone position from multiple landmark observations."""
if len(detected) < 2:
return None
positions = []
weights = []
for det in detected:
if det['id'] in self.landmark_database:
landmark_pos = self.landmark_database[det['id']]['position']
offset = det['position']
# Compute drone position
drone_pos = landmark_pos - offset
positions.append(drone_pos)
# Weight by inverse distance
weights.append(1.0 / (det['distance'] + 0.1))
if not positions:
return None
positions = np.array(positions)
weights = np.array(weights)
weights = weights / weights.sum()
# Weighted average
position = np.average(positions, axis=0, weights=weights)
return position
def create_fiducial_map(self, room_corners: List[np.ndarray],
spacing: float = 2.0):
"""Automatically place landmarks in a grid pattern."""
min_corner = np.min(room_corners, axis=0)
max_corner = np.max(room_corners, axis=0)
landmark_id = 0
x = min_corner[0]
while x <= max_corner[0]:
y = min_corner[1]
while y <= max_corner[1]:
position = np.array([x, y, min_corner[2] + 2.5]) # Ceiling height
self.register_landmark(landmark_id, position)
landmark_id += 1
y += spacing
x += spacing
return landmark_id # Total landmarks placed
Sensor Fusion for Indoor Navigation
Combine multiple positioning systems for robust indoor navigation.
python
from typing import Dict, List
class IndoorNavigationFusion:
def __init__(self):
self.vio = None
self.uwb = None
self.wifi = None
self.visual_landmarks = None
self.position = np.zeros(3)
self.velocity = np.zeros(3)
self.covariance = np.eye(6) * 0.1
# Fusion weights (adaptive based on confidence)
self.weights = {
'vio': 0.3,
'uwb': 0.4,
'wifi': 0.15,
'visual': 0.15
}
def update(self, measurements: Dict):
"""Fuse measurements from multiple sources."""
positions = []
confidences = []
# VIO
if 'vio' in measurements:
vio_pos = measurements['vio']
positions.append(vio_pos)
confidences.append(self.weights['vio'])
# UWB
if 'uwb' in measurements:
uwb_pos = measurements['uwb']
positions.append(uwb_pos)
confidences.append(self.weights['uwb'])
# WiFi
if 'wifi' in measurements:
wifi_pos = measurements['wifi']
positions.append(wifi_pos)
confidences.append(self.weights['wifi'])
# Visual landmarks
if 'visual' in measurements:
visual_pos = measurements['visual']
positions.append(visual_pos)
confidences.append(self.weights['visual'])
if not positions:
return self.position
# Weighted fusion
positions = np.array(positions)
confidences = np.array(confidences)
confidences = confidences / confidences.sum()
# Fused position
fused_position = np.average(positions, axis=0, weights=confidences)
# Update covariance estimate
if len(positions) > 1:
position_variance = np.var(positions, axis=0)
self.covariance[:3, :3] = np.diag(position_variance)
self.position = fused_position
return self.position
def get_navigation_state(self) -> Dict:
"""Get current navigation state."""
return {
'position': self.position.copy(),
'velocity': self.velocity.copy(),
'position_uncertainty': np.sqrt(np.diag(self.covariance[:3, :3])),
'active_sensors': self._get_active_sensors()
}
def _get_active_sensors(self) -> List[str]:
active = []
if self.vio is not None:
active.append('VIO')
if self.uwb is not None:
active.append('UWB')
if self.wifi is not None:
active.append('WiFi')
if self.visual_landmarks is not None:
active.append('Visual')
return active
def adapt_weights(self, sensor_performance: Dict):
"""Adapt fusion weights based on sensor performance."""
for sensor, performance in sensor_performance.items():
if sensor in self.weights:
# Increase weight for good performance
self.weights[sensor] *= (1 + performance * 0.1)
# Normalize
total = sum(self.weights.values())
self.weights = {k: v/total for k, v in self.weights.items()}
Hands-On Project: Complete Indoor Navigation System
python
from typing import Dict, List, Tuple
class IndoorDroneNavigator:
def __init__(self, room_dimensions: Tuple[float, float, float]):
self.room = room_dimensions
# Positioning systems
anchors = UWBAnchorPlacement.optimize_placement(room_dimensions)
self.uwb = UWBPositioning(anchors)
self.vio = None # Initialize with camera params
self.wifi = WiFiPositioning()
# Fusion
self.fusion = IndoorNavigationFusion()
# Mapping
self.occupancy_grid = np.zeros((
int(room_dimensions[0] / 0.5),
int(room_dimensions[1] / 0.5)
))
# Path planning
self.current_path = []
self.waypoint_index = 0
def initialize Systems(self):
"""Initialize all positioning systems."""
# Register visual landmarks
self.visual_system = VisualLandmarkSystem(
camera_matrix=np.eye(3),
dist_coeffs=None
)
self.visual_system.create_fiducial_map(
[np.array([0, 0, 0]), np.array(self.room)]
)
def navigate_to(self, target: np.ndarray) -> Dict:
"""Navigate to target position indoors."""
state = self.fusion.get_navigation_state()
# Simple proportional navigation
error = target - state['position']
distance = np.linalg.norm(error)
if distance < 0.3:
return {'status': 'arrived', 'position': state['position']}
# Compute velocity command
velocity = error * 0.5 # Proportional control
velocity = np.clip(velocity, -2, 2)
return {
'status': 'navigating',
'velocity': velocity,
'distance_to_target': distance,
'position': state['position'],
'uncertainty': state['position_uncertainty']
}
def update_obstacle_map(self, lidar_scan: np.ndarray,
robot_position: np.ndarray):
"""Update occupancy grid from sensor data."""
for angle, distance in zip(np.linspace(-np.pi, np.pi, 360), lidar_scan):
if distance < 10: # Max range
# Convert to grid coordinates
x = int((robot_position[0] + distance * np.cos(angle)) / 0.5)
y = int((robot_position[1] + distance * np.sin(angle)) / 0.5)
if 0 <= x < self.occupancy_grid.shape[0] and \
0 <= y < self.occupancy_grid.shape[1]:
self.occupancy_grid[x, y] = 1 # Occupied
def plan_path_astar(self, start: np.ndarray, goal: np.ndarray) -> List[np.ndarray]:
"""A* path planning on occupancy grid."""
# Simplified A* implementation
start_grid = (int(start[0] / 0.5), int(start[1] / 0.5))
goal_grid = (int(goal[0] / 0.5), int(goal[1] / 0.5))
# ... (A* implementation similar to previous tutorials)
# Return path in world coordinates
path = []
# Placeholder: straight line path
n_points = 20
for i in range(n_points):
t = i / (n_points - 1)
point = start + t * (goal - start)
path.append(point)
return path
def main():
# Initialize indoor navigation system
room = (20.0, 15.0, 4.0) # 20m x 15m x 4m room
navigator = IndoorDroneNavigator(room)
# Simulate navigation
start = np.array([2.0, 2.0, 1.5])
target = np.array([18.0, 13.0, 1.5])
# Plan path
path = navigator.plan_path_astar(start, target)
print(f"Planned path with {len(path)} waypoints")
# Navigate
for i, waypoint in enumerate(path):
result = navigator.navigate_to(waypoint)
print(f"Waypoint {i}: {result['status']}")
if result['status'] == 'arrived':
continue
if __name__ == "__main__":
main()
Key Takeaways
Advertisement
- VIO provides drift-aware odometry without external references
- UWB offers high-accuracy positioning with proper anchor placement
- WiFi fingerprinting leverages existing infrastructure for coarse positioning
- Visual landmarks give absolute position fixes
- Sensor fusion combines systems for robust indoor navigation
- Always handle multipath and signal degradation in radio-based systems