# IMUPositionApproximator (`assignments::three::g2_2025_imu_position_approximator_node`)

The `IMUPositionApproximator` node calculates position and velocity estimates by integrating
acceleration data from an IMU sensor. It transforms sensor data from the robot frame to the
map frame, accounting for rotations and centripetal effects, and provides a position reset
topic.

## Implementation Details

**Parameters**

| Parameter | Type | Default | Description |
|-----------|------|---------|-------------|
| `initial_x` | double | 0.0 | Initial x position in map frame |
| `initial_y` | double | 0.0 | Initial y position in map frame |
| `initial_z` | double | 0.0 | Initial z position in map frame |
| `initial_theta` | double | 0.0 | Initial orientation angle (radians) |
| `imu_topic` | string | "imu_data" | Topic name for IMU sensor data |

**Constructor**
```cpp
IMUPositionApproximator()
```
- Initializes ROS2 node with name `imu_position_approximator`
- Creates subscription to IMU topic (configurable) and `position_reset` topic
- Creates publishers for `estimated_position` and `estimated_velocity`

## Functions

**`void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg)`**
- Primary callback invoked whenever an IMU message is received
- Calculates time delta `dt` between measurements; skips invalid deltas (≤0 or >1 second)
- Updates orientation: `theta += omega_z * dt` and normalizes to [-pi, pi]
- Applies gravity compensation: subtracts 9.81 m/s^2 from z-axis acceleration
- Transforms acceleration from robot frame to map frame using rotation matrix
- Corrects for centripetal acceleration due to rotation
- Integrates acceleration to update velocity, then integrates velocity to update position
- Publishes `estimated_position` (Pose2D) and `estimated_velocity` (Twist)

**`void position_reset_callback(const geometry_msgs::msg::Pose2D::SharedPtr msg)`**
- Updates position (x, y) and orientation (theta) to provided values
- Resets all velocities (vx, vy, vz) to zero

## ROS2 Interface

**Subscribers**
- `imu_data` (sensor_msgs/msg/Imu) [configurable via `imu_topic` parameter]
  - Receives raw IMU sensor data with linear acceleration and angular velocity

- `position_reset` (geometry_msgs/msg/Pose2D)
  - Receives position reset commands from external position determinator

**Publishers**
- `estimated_position` (geometry_msgs/msg/Pose2D)
  - Publishes calculated position (x, y, theta) in map frame

- `estimated_velocity` (geometry_msgs/msg/Twist)
  - Publishes calculated linear velocities (x, y, z) and angular velocity (z)