{"id":29408766,"url":"https://github.com/ikajdan/state-estimation-factor-graphs","last_synced_at":"2025-07-26T09:34:38.552Z","repository":{"id":245172993,"uuid":"796248678","full_name":"ikajdan/state-estimation-factor-graphs","owner":"ikajdan","description":"State Estimation of a Differential Robot using Factor Graphs","archived":false,"fork":false,"pushed_at":"2024-05-11T20:25:33.000Z","size":6044,"stargazers_count":1,"open_issues_count":0,"forks_count":1,"subscribers_count":1,"default_branch":"main","last_synced_at":"2025-07-23T23:45:24.010Z","etag":null,"topics":["amcl","gazebo","gtsam","kalman-filter","odometry","ros2","slam"],"latest_commit_sha":null,"homepage":"","language":"Python","has_issues":true,"has_wiki":null,"has_pages":null,"mirror_url":null,"source_name":null,"license":"mit","status":null,"scm":"git","pull_requests_enabled":true,"icon_url":"https://github.com/ikajdan.png","metadata":{"files":{"readme":"README.md","changelog":null,"contributing":null,"funding":null,"license":"LICENSE.md","code_of_conduct":null,"threat_model":null,"audit":null,"citation":null,"codeowners":null,"security":null,"support":null,"governance":null,"roadmap":null,"authors":null,"dei":null,"publiccode":null,"codemeta":null}},"created_at":"2024-05-05T11:49:16.000Z","updated_at":"2025-06-24T11:40:23.000Z","dependencies_parsed_at":"2024-06-20T08:59:18.765Z","dependency_job_id":"7fd68510-f813-4c58-bafc-a415ed8fe913","html_url":"https://github.com/ikajdan/state-estimation-factor-graphs","commit_stats":null,"previous_names":["ikajdan/state_estimation_factor_graphs","ikajdan/state-estimation-factor-graphs"],"tags_count":0,"template":false,"template_full_name":null,"purl":"pkg:github/ikajdan/state-estimation-factor-graphs","repository_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/ikajdan%2Fstate-estimation-factor-graphs","tags_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/ikajdan%2Fstate-estimation-factor-graphs/tags","releases_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/ikajdan%2Fstate-estimation-factor-graphs/releases","manifests_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/ikajdan%2Fstate-estimation-factor-graphs/manifests","owner_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners/ikajdan","download_url":"https://codeload.github.com/ikajdan/state-estimation-factor-graphs/tar.gz/refs/heads/main","sbom_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/ikajdan%2Fstate-estimation-factor-graphs/sbom","host":{"name":"GitHub","url":"https://github.com","kind":"github","repositories_count":267145717,"owners_count":24042651,"icon_url":"https://github.com/github.png","version":null,"created_at":"2022-05-30T11:31:42.601Z","updated_at":"2022-07-04T15:15:14.044Z","status":"online","status_checked_at":"2025-07-26T02:00:08.937Z","response_time":62,"last_error":null,"robots_txt_status":"success","robots_txt_updated_at":"2025-07-24T06:49:26.215Z","robots_txt_url":"https://github.com/robots.txt","online":true,"can_crawl_api":true,"host_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub","repositories_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories","repository_names_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repository_names","owners_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners"}},"keywords":["amcl","gazebo","gtsam","kalman-filter","odometry","ros2","slam"],"created_at":"2025-07-11T03:05:46.204Z","updated_at":"2025-07-26T09:34:38.509Z","avatar_url":"https://github.com/ikajdan.png","language":"Python","funding_links":[],"categories":[],"sub_categories":[],"readme":"# State Estimation using Factor Graphs\n\nThis repository contains a robot state estimation using factor graphs. The demo consists of a simple robot that moves in a 2D space. It is equipped with sensors for odometry and localization.\n\nThe robot is controlled using a differential drive system, implemented with the Navigation2 stack. The state estimation is done using the GTSAM library. For comparison, the AMCL (Adaptive Monte Carlo Localization) localization node is also used. The simulation is done in Gazebo.\n\n\u003cbr\u003e\n\u003cdiv align=\"center\"\u003e\n  \u003cimg src=\"docs/demo.gif\"/\u003e\n  \u003cbr\u003e\u003cbr\u003e\n  \u003cem\u003eRobot state estimation (red vector: AMCL, blue: GTSAM).\u003c/em\u003e\n\u003c/div\u003e\n\u003cbr\u003e\n\n## Project Structure\n\nThe project is divided into two packages:\n\n- `robot_simulation`: contains the xacro definition files of the robot, the AMCL localization, and navigation node launch files, and the Gazebo world files.\n- `state_estimation`: contains the GTSAM state estimation node.\n\n### Robot Simulation Package\n\nThe robot is equipped with a lidar, an IMU (Inertial Measurement Unit), and wheel encoders. An EKF (Extended Kalman Filter) from the `robot_localization` package is used to fuse the odometry and IMU data to estimate the robot's pose.\n\nThe data collected from the lidar and the odometry sensors is used by the AMCL node to localize the robot in the environment. The AMCL node subscribes to the laser scan data (`/scan`) and the non-filtered odometry data (`/diff_cont/odom`) and publishes the estimated pose of the robot (`/amcl_pose`).\n\nThe robot is controlled using the Navigation2 stack. The `diff_drive_controller` is used to control the robot's velocity. The `nav2_controller` is used to navigate the robot to a goal position.\n\nThe description of the robot is defined in the `description` directory. The `launch` directory contains the launch files for the simulation and the localization node. The `maps` directory contains the map of the environment. The `worlds` directory contains the Gazebo world files. Configuration files are stored in the `config` directory.\n\n\u003cbr\u003e\n\u003cdiv align=\"center\"\u003e\n  \u003cimg src=\"docs/robot.png\" width=\"45%\"\u003e\n\u0026nbsp; \u0026nbsp; \u0026nbsp; \u0026nbsp;\n  \u003cimg src=\"docs/gazebo_world.png\" width=\"45%\"\u003e\n  \u003cbr\u003e\u003cbr\u003e\n  \u003cem\u003eCustom robot model and the Gazebo world.\u003c/em\u003e\n\u003c/div\u003e\n\u003cbr\u003e\n\n### State Estimation Package\n\nThe `state_estimation` package contains a custom node that uses the GTSAM library to perform robot position estimation. The node subscribes to pose (`/amcl_pose`) and odometry (`/odometry/filtered`) topics, processes the data using GTSAM's factor graphs and optimization algorithms, and then publishes the pose estimate (`/gtsam_pose`).\n\n## Set up the Environment\n\nTo run the simulation, you will need to have Docker installed on your machine. The project was developed using Docker version 26.1.1.\n\n1. Clone the repository:\n\n    ```bash\n    git clone git@github.com:ikajdan/ros_differential_robot_simulation.git\n    ```\n\n2. Build the Docker image:\n\n    ```bash\n    ./build.sh\n    ```\n\n3. Run the container:\n\n    ```bash\n    ./run.sh\n    ```\n\n4. Build the workspace:\n\n    ```bash\n    colcon build --symlink-install\n    ```\n\n## Run the Simulation\n\nTo run the simulation, follow these steps:\n\n1. Source the workspace:\n\n    ```bash\n    source ./install/setup.bash\n    ```\n\n2. Launch the simulation:\n\n    ```bash\n    ros2 launch robot_simulation sim.launch.py world:=./src/robot_simulation/worlds/main.world gui:=false\n    ```\n\n\u003e [!NOTE]\n\u003e The initial startup of the simulation may take a while.\n\n\u003e [!TIP]\n\u003e To visualize the Gazebo simulation, omit the `gui:=false` argument.\n\n3. Launch the AMCL localization node:\n\n    ```bash\n    ros2 launch robot_simulation localization.launch.py map:=./src/robot_simulation/maps/main.yaml\n    ```\n\n4. Launch the GTSAM state estimation node:\n    ```bash\n    ros2 run state_estimation state_estimation\n    ```\n\n5. (Optional) Run the navigation node:\n\n    ```bash\n    ros2 launch robot_simulation navigation.launch.py\n    ```\n\n6. Launch the RViz visualization tool:\n\n    ```bash\n    ros2 run rviz2 rviz2 -d ./src/robot_simulation/config/view_main.rviz --ros-args -p use_sim_time:=true\n    ```\n\n7. Set the initial pose of the robot in RViz.\n\n\u003cbr\u003e\n\u003cdiv align=\"center\"\u003e\n  \u003cimg src=\"docs/map_loaded.png\" width=\"45%\"\u003e\n\u0026nbsp; \u0026nbsp; \u0026nbsp; \u0026nbsp;\n  \u003cimg src=\"docs/setup_complete.png\" width=\"45%\"\u003e\n  \u003cbr\u003e\u003cbr\u003e\n  \u003cem\u003eInitial pose set in RViz.\u003c/em\u003e\n\u003c/div\u003e\n\u003cbr\u003e\n\n\u003e [!NOTE]\n\u003e Make sure to set the fixed frame to `map` in RViz.\n\n## World Mapping\n\nTo create a map of the environment, use the `slam_toolbox` package:\n\n```bash\nros2 launch robot_simulation slam.launch.py\nros2 run rviz2 rviz2 -d ./src/robot_simulation/config/view_map.rviz --ros-args -p use_sim_time:=true\n```\n\nThis will launch the `slam_toolbox` node and RViz. Use the `teleop_twist_keyboard` package to move the robot around the environment and create the map:\n\n```bash\nros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r /cmd_vel:=/diff_cont/cmd_vel_unstamped\n```\n\n\u003cbr\u003e\n\u003cdiv align=\"center\"\u003e\n  \u003cimg src=\"docs/world_mapping.png\" width=\"45%\"\u003e\n\u0026nbsp; \u0026nbsp; \u0026nbsp; \u0026nbsp;\n  \u003cimg src=\"docs/map_complete.png\" width=\"45%\"\u003e\n  \u003cbr\u003e\u003cbr\u003e\n  \u003cem\u003eMapping the environment using the \"slam_toolbox\" package.\u003c/em\u003e\n\u003c/div\u003e\n\u003cbr\u003e\n\nOnce the map is created, save it using the dialog in RViz to the [maps](src/robot_simulation/maps) directory. The generated map will be saved in the `pgm` and `yaml` format.\n\n\u003cbr\u003e\n\u003cdiv align=\"center\"\u003e\n  \u003cimg src=\"docs/map.png\"\u003e\n  \u003cbr\u003e\u003cbr\u003e\n  \u003cem\u003eGenerated map of the environment.\u003c/em\u003e\n\u003c/div\u003e\n\u003cbr\u003e\n\n## References\n\nThe wast majority of the robot simulation code is based on the [Building a Mobile Robot](https://www.youtube.com/playlist?list=PLunhqkrRNRhYAffV8JDiFOatQXuU-NnxT) tutorial made by [Articulated Robotics](https://www.youtube.com/@ArticulatedRobotics).\n\nThe code for the GTSAM state estimation node is based on the following tutorial: [Factor Graphs and GTSAM](https://gtsam.org/tutorials/intro.html).\n\nOther resources used in the development of this project include:\n- https://automaticaddison.com/sensor-fusion-using-the-robot-localization-package-ros-2/\n- https://classic.gazebosim.org/tutorials?tut=ros_gzplugins\n- https://navigation.ros.org/setup_guides/sensors/setup_sensors.html\n\n\n## License\n\nThis repository is licensed under the MIT License - see the [LICENSE](LICENSE) file for details.\n","project_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fikajdan%2Fstate-estimation-factor-graphs","html_url":"https://awesome.ecosyste.ms/projects/github.com%2Fikajdan%2Fstate-estimation-factor-graphs","lists_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fikajdan%2Fstate-estimation-factor-graphs/lists"}