Adding localization feature

templates/localization/docs/GRID_SYSTEM.md

@@ -0,0 +1,41 @@
+# Field Grid System
+
+## Grid Layout
+
+12x12 cells, each 12" x 12":
+
+```
+    0   1   2   3   4   5   6   7   8   9  10  11
+11  .   .   .   .   .   .   .   .   .   .   .   B
+10  .   .   .   .   .   .   .   .   .   .   .   .
+ 9  .   .   .   .   .   .   .   .   .   .   .   .
+ 8  .   .   .   .   .   .   .   .   .   .   .   .
+ 7  .   .   .   .   .   .   .   .   .   .   .   .
+ 6  .   .   .   .   .   .   X   .   .   .   .   .
+ 5  .   .   .   .   .   .   .   .   .   .   .   .
+ 4  .   .   .   .   .   .   .   .   .   .   .   .
+ 3  .   .   .   .   .   .   .   .   .   .   .   .
+ 2  .   .   .   .   .   .   .   .   .   .   .   .
+ 1  .   .   .   .   .   .   .   .   .   .   .   .
+ 0  R   .   .   .   .   .   .   .   .   .   .   .
+
+R = Red backstage (0,0)
+B = Blue backstage (11,11)
+X = Center (6,6)
+```
+
+## Usage
+
+```java
+GridCell cell = new GridCell(5, 7);
+double dist = cell.distanceTo(FieldGrid.CENTER);
+double angle = cell.angleTo(FieldGrid.BLUE_BACKSTAGE);
+```
+
+## Common Locations
+
+```java
+FieldGrid.RED_BACKSTAGE    // (0, 0)
+FieldGrid.BLUE_BACKSTAGE   // (11, 11)
+FieldGrid.CENTER           // (6, 6)
+```

templates/localization/docs/LOCALIZATION_GUIDE.md

@@ -0,0 +1,72 @@
+# Robot Localization Guide
+
+## What is Localization?
+
+Answering: "Where is my robot on the field?"
+
+## The Grid System
+
+12ft x 12ft field → 12x12 grid of 12" cells
+
+```
+Cell (0,0) = Red backstage
+Cell (11,11) = Blue backstage
+Cell (6,6) = Center
+```
+
+## Sensor Fusion
+
+Combine three sensors:
+
+1. **Odometry (Encoders)** - Track wheel rotation
+   - Accuracy: ±1" per foot (cumulative drift)
+   - Always available
+
+2. **IMU (Gyroscope)** - Measure heading
+   - Accuracy: ±2° (non-cumulative)
+   - Corrects heading drift
+
+3. **Vision (AprilTags)** - Detect position markers
+   - Accuracy: ±2" (when visible)
+   - Ground truth - resets drift
+
+## Fusion Strategy
+
+```
+if vision available:
+    position = vision (most accurate)
+    correct odometry
+elif IMU available:
+    position = odometry
+    heading = IMU
+else:
+    position = odometry only (dead reckoning)
+```
+
+## Fault Tolerance
+
+| Sensors | Accuracy | Confidence |
+|---------|----------|------------|
+| All 3 | ±2" | 100% |
+| Odometry + IMU | ±4" | 70% |
+| Odometry only | ±12" | 40% |
+
+System keeps working when sensors fail!
+
+## Usage
+
+```java
+RobotLocalizer localizer = new RobotLocalizer(odometry, imu, vision);
+localizer.setInitialPose(new Pose2D(12, 12, 0));
+
+while (opModeIsActive()) {
+    localizer.update();
+    
+    GridCell cell = localizer.getCurrentCell();
+    double confidence = localizer.getConfidence();
+    
+    // Make decisions based on position
+}
+```
+
+See README.md for full examples.