Controller Update

2026-02-09 05:51:51 +00:00
parent cd9ae9a4f6
commit 1a616472f0
16 changed files with 1545 additions and 669 deletions
--- a/docs/gps_denied_navigation.md
+++ b/docs/gps_denied_navigation.md
@@ -1,128 +1,211 @@
 # GPS-Denied Navigation

-How the system navigates without GPS.
+## Overview

-## Principle
+This system enables UAV/UGV navigation **without GPS** by using:

-All navigation uses relative positioning from visual sensors. GPS is only used for geofencing (safety boundaries).
+1. **Visual Odometry** - Camera-based pose estimation
+2. **Optical Flow** - Velocity estimation from downward camera
+3. **IMU Integration** - Short-term dead reckoning
+4. **EKF Fusion** - Combine all sensors

-| Function | GPS Used? |
-|----------|-----------|
-| Position estimation | No - visual odometry |
-| Waypoint navigation | No - local coordinates |
-| Velocity control | No - optical flow |
-| Geofencing | Yes - safety only |
+**GPS is ONLY used for geofencing (safety boundaries).**

-## Position Estimation
+## How It Works

 ### Visual Odometry

-1. Detect features in camera image (ORB, SIFT)
-2. Match features between consecutive frames
-3. Estimate camera motion from feature displacement
-4. Accumulate motion into position estimate
+```
+Frame N-1    Frame N
+    │           │
+    ▼           ▼
+┌────────┐  ┌────────┐
+│Features│──│Features│  → Match features
+└────────┘  └────────┘
+    │           │
+    └─────┬─────┘
+          ▼
+    Essential Matrix → Rotation + Translation
+```
+
+**Algorithm:**
+1. Detect ORB/SIFT features in current frame
+2. Match with previous frame features
+3. Compute Essential Matrix (RANSAC)
+4. Recover rotation and translation
+5. Integrate to get absolute pose

 ### Optical Flow

-1. Capture ground images from downward camera
-2. Measure pixel displacement between frames
-3. Convert to velocity using altitude
-4. Integrate for position
+```
+Downward Camera
+      │
+      ▼
+  ┌───────┐
+  │ Image │ → Lucas-Kanade flow
+  └───────┘
+      │
+      ▼
+  Pixel velocity × Altitude / Focal length = Ground velocity
+```
+
+**Works best at:**
+- Altitudes 0.5m - 10m
+- Textured ground surfaces
+- Stable lighting

 ### Sensor Fusion

-Extended Kalman Filter combines:
- Visual odometry (position)
- Optical flow (velocity)
- IMU (acceleration, rotation)
- Barometer (altitude)
+```
+Visual Odometry ─┬─→ Weighted Average ─→ Position Estimate
+Optical Flow ────┤
+IMU ─────────────┘
+```

-Output: Full 6-DOF pose estimate
+**Weights (configurable):**
+- Visual Odometry: 60%
+- Optical Flow: 30%
+- IMU: 10%

 ## ArduPilot Configuration

-Key parameters for GPS-denied operation:
+### EKF3 External Navigation

 ```
-# EKF Source Configuration
-EK3_SRC1_POSXY = 6    # External Nav for position
-EK3_SRC1_VELXY = 6    # External Nav for velocity
-EK3_SRC1_POSZ = 1     # Barometer for altitude
+# GPS Type - Disabled
+GPS_TYPE 0
+GPS_TYPE2 0

-# Disable GPS for navigation
-GPS_TYPE = 0          # No GPS (or keep for geofence)
+# EKF3 Source Configuration
+AHRS_EKF_TYPE 3      # Use EKF3
+EK3_ENABLE 1
+EK2_ENABLE 0

-# Enable external navigation
-VISO_TYPE = 1         # Enable visual odometry input
+# Position from External Nav
+EK3_SRC1_POSXY 6     # External Nav
+EK3_SRC1_POSZ 1      # Barometer
+EK3_SRC1_VELXY 6     # External Nav
+EK3_SRC1_VELZ 0      # None
+EK3_SRC1_YAW 6       # External Nav

-# Arming checks
-ARMING_CHECK = 0      # Disable pre-arm checks (for testing)
+# Vision Position Input
+VISO_TYPE 1          # MAVLink
+VISO_POS_X 0.1       # Camera offset
+VISO_DELAY_MS 50     # Processing delay
 ```

-See `config/ardupilot_gps_denied.parm` for complete parameters.
+### Arming Checks

-## Sending Position to ArduPilot
+```
+# For simulation, disable all
+ARMING_CHECK 0

-Visual odometry sends position via MAVLink:
+# For real flight, keep safety checks
+# ARMING_CHECK 14  # Skip GPS only
+```
+
+## Coordinate Frames
+
+### Local NED Frame
+
+```
+        North (X+)
+           ↑
+           │
+           │
+West ←─────┼─────→ East (Y+)
+           │
+           │
+           ↓
+        (Down is Z+)
+```
+
+**All navigation uses LOCAL coordinates:**
+- Takeoff point is origin (0, 0, 0)
+- No global GPS coordinates
+- Relative waypoints only
+
+### Example Waypoints

 ```python
-# VISION_POSITION_ESTIMATE message
-msg = mavutil.mavlink.MAVLink_vision_position_estimate_message(
-    usec=timestamp_us,
-    x=position_x,      # meters, NED frame
-    y=position_y,
-    z=position_z,
-    roll=roll,         # radians
-    pitch=pitch,
-    yaw=yaw
-)
-```
-
-## Drift Mitigation
-
-Visual odometry accumulates drift over time. Strategies:
-
-1. **Loop Closure**: Recognize previously visited locations
-2. **Landmark Matching**: Use known visual markers
-3. **Multi-Sensor Fusion**: Weight sensors by confidence
-4. **Periodic Reset**: Return to known position
-
-## Geofencing
-
-GPS is only used for safety boundaries:
-
-```yaml
-geofence:
-  enabled: true
-  use_gps: true
-  fence_type: polygon
-  action: RTL
-  max_altitude: 50
-```
-
-If drone crosses boundary, triggers return-to-launch.
-
-## Coordinate System
-
-All waypoints use local NED coordinates:
- X: North (meters from origin)
- Y: East (meters from origin)
- Z: Down (negative for altitude)
-
-Example mission:
-```python
+# Square pattern (5m sides)
 waypoints = [
-    {"x": 0, "y": 0, "z": -5},    # Takeoff to 5m
-    {"x": 10, "y": 0, "z": -5},   # 10m north
-    {"x": 10, "y": 10, "z": -5},  # 10m east
-    {"x": 0, "y": 0, "z": -5},    # Return
-    {"x": 0, "y": 0, "z": 0},     # Land
+    (5, 0, -5),    # North 5m, altitude 5m
+    (5, 5, -5),    # North-East corner
+    (0, 5, -5),    # East
+    (0, 0, -5),    # Back to start
 ]
 ```

 ## Limitations

- Drift accumulates over distance/time
- Requires visual features (fails in featureless environments)
- Requires sufficient lighting
- Performance degrades with fast motion or blur
+### Visual Odometry
+- **Scale drift**: Position error grows over time
+- **Texture needed**: Poor in featureless environments
+- **Lighting**: Affected by shadows, brightness changes
+- **Motion blur**: High-speed motion degrades accuracy
+
+### Optical Flow
+- **Altitude dependent**: Accuracy varies with height
+- **Ground texture**: Needs visible ground features
+- **Tilt sensitivity**: Assumes mostly horizontal flight
+
+### Mitigation Strategies
+
+1. **Loop closure**: Return to known positions
+2. **Landmark detection**: ArUco markers for correction
+3. **Multi-sensor fusion**: Combine VO + OF + IMU
+4. **Flight patterns**: Minimize cumulative drift
+
+## Geofencing (GPS Only)
+
+GPS is used **only** for safety:
+
+```python
+# Geofence uses GPS coordinates
+fence_points = [
+    (47.397742, 8.545594),  # Corner 1 (lat, lon)
+    (47.398242, 8.545594),  # Corner 2
+    (47.398242, 8.546094),  # Corner 3
+    (47.397742, 8.546094),  # Corner 4
+]
+
+# But navigation uses local coordinates
+current_position = (10.5, 3.2, -5.0)  # NED, meters
+```
+
+**Breach Actions:**
+- `RTL` - Return to local origin
+- `LAND` - Land immediately
+- `HOLD` - Hold position
+
+## Testing GPS-Denied Mode
+
+### 1. Verify EKF Source
+
+In MAVProxy:
+```
+param show EK3_SRC*
+```
+
+Should show:
+```
+EK3_SRC1_POSXY    6.0
+EK3_SRC1_VELXY    6.0
+```
+
+### 2. Check Vision Input
+
+```bash
+ros2 topic echo /uav/visual_odometry/pose
+```
+
+Should show updating position.
+
+### 3. Monitor EKF Status
+
+```bash
+ros2 topic echo /uav/mavros/state
+```
+
+Should show `connected: true` and `mode: GUIDED`.