diff --git a/Makefile b/Makefile
new file mode 100644
index 0000000..df6b74b
--- /dev/null
+++ b/Makefile
@@ -0,0 +1,32 @@
+DATA_DIR=/data
+SSH_PRIV_KEY=`cat ~/.ssh/id_rsa`
+
+help: ## Help
+	@echo -e "\033[1;34m[make targets]:\033[0m"
+	@egrep -h '\s##\s' $(MAKEFILE_LIST) \
+		| awk 'BEGIN {FS = ":.*?## "}; \
+		{printf "\033[1;36m%-20s\033[0m %s\n", $$1, $$2}'
+
+install_docker: ## Install Docker (Ubuntu only)
+	@bash scripts/install_docker.bash
+
+build_docker:  ## Build srl_mav_sim docker
+	cd docker && ./build_docker_image.bash
+
+run_docker: ## Run srl_mav_sim docker
+	@xhost +local:docker && docker run \
+		-e DISPLAY \
+		-v $(DATA_DIR):$(DATA_DIR) \
+		-v /dev/bus/usb:/dev/bus/usb --device-cgroup-rule='c 189:* rmw' \
+		-v /dev/shm:/dev/shm \
+		-v /dev/dri:/dev/dri \
+		--privileged \
+		--ipc=host \
+		--pid=host \
+		--network="host" \
+		-it --rm srl_mav_sim /bin/bash
+
+fix_px4: ## Fix PX4 in gazebo mode
+	rosrun mavros mavparam set COM_RCL_EXCEPT 4  # RC LOSS EXCEPTION -> 4 (Not documented)
+	rosrun mavros mavparam set NAV_DLL_ACT 0     # GCS loss failsafe mode -> 0 (Disabled)
+	rosrun mavros mavparam set NAV_RCL_ACT 0     # RC loss failsafe modea -> 0 (Not documented)
diff --git a/README.md b/README.md
index 792ad2a..3c97b7c 100644
--- a/README.md
+++ b/README.md
@@ -5,6 +5,20 @@ The simulators have been tested on Ubuntu 20.04 using ROS Noetic.
 
 ## Setup
 
+If you're lazy like me you can run the entire simulator in docker
+mode via three make targets:
+
+```
+make install_docker # Install docker
+make build_docker   # Build docker container
+make run_docker     # Run docker container
+# Once you're running the container, both the username and password is `docker`
+```
+
+---
+
+Alternatively ...
+
 Install the `ros-noetic-desktop-full` package by following the instructions
 from [here](http://wiki.ros.org/noetic/Installation) and then install the
 common dependencies.
@@ -47,6 +61,25 @@ build and use the MAV simulator based on Ignition Gazebo. See
 linear MPC.
 
 
+## Important Note
+
+If the drone does not seem to be taking off in Gazebo, you will probably need
+to set the following PX4 parameters
+([source](https://github.com/PX4/PX4-Autopilot/issues/19919#issuecomment-1188864384)):
+
+```
+rosrun mavros mavparam set COM_RCL_EXCEPT 4  # RC LOSS EXCEPTION -> 4 (Not documented)
+rosrun mavros mavparam set NAV_DLL_ACT 0     # GCS loss failsafe mode -> 0 (Disabled)
+rosrun mavros mavparam set NAV_RCL_ACT 0     # RC loss failsafe modea -> 0 (Not documented)
+```
+
+Or simply type `make fix_px4` in the terminal. Then you can test out whether
+the drone can take off by running:
+
+```
+python3 scripts/test_offboard_mode.py  # Sends offboard + arm + position setpoint command
+```
+
 ## MAVROS
 
 [MAVROS](http://wiki.ros.org/mavros) allows communicating with the PX4 through
diff --git a/docker/Dockerfile b/docker/Dockerfile
new file mode 100644
index 0000000..88bc5ae
--- /dev/null
+++ b/docker/Dockerfile
@@ -0,0 +1,160 @@
+FROM ubuntu:focal
+SHELL ["/bin/bash", "-c"]
+
+# Settings
+ENV ARCH=amd64
+ENV DEBIAN_FRONTEND=noninteractive
+ARG ROS_DISTRO=noetic
+
+# Permissions
+RUN apt-get update && apt-get install -y sudo
+ARG USER=docker
+ARG PASS=docker
+ARG UID=1000
+ARG GID=1000
+ARG SSH_PRIV_KEY
+RUN useradd -m ${USER} --uid=${UID} && echo "${USER}:${PASS}" | chpasswd
+RUN adduser ${USER} sudo
+ENV HOME /home/$USER
+
+# Install deps
+RUN apt-get install -qy \
+  git \
+  tree \
+  htop \
+  tmux \
+  exuberant-ctags \
+  automake \
+  cmake \
+  ccache \
+  gcc \
+  clang \
+  clang-format \
+  clang-tidy \
+  protobuf-compiler \
+  libpython3-dev \
+  pylint \
+  yapf3
+
+# Install ROS
+RUN apt-get install -y lsb-release
+RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
+RUN apt-get install curl gnupg -qy
+RUN curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
+RUN apt-get update
+RUN apt-get install ros-${ROS_DISTRO}-desktop -qy
+
+# Install Gazebo-Fortress
+RUN apt-get install wget -qy
+RUN wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg
+RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null
+RUN apt-get update
+RUN apt-get install ignition-fortress -qy
+
+# Install MAVROS
+RUN apt-get install ros-${ROS_DISTRO}-mavros ros-${ROS_DISTRO}-mavros-extras -qy
+
+# Install additional tools and libraries
+RUN apt-get install -qy \
+  python3-pip \
+  python3-numpy \
+  python3-scipy \
+  python3-pandas \
+  python3-seaborn \
+  python3-matplotlib \
+  python3-setuptools \
+  python3-catkin-tools \
+  libeigen3-dev \
+  libopencv-dev
+
+# Switch to user
+USER ${USER}
+
+# Setup catkin workspace
+WORKDIR $HOME
+RUN mkdir -p $HOME/catkin_ws/src
+RUN cd $HOME/catkin_ws \
+  && source /opt/ros/${ROS_DISTRO}/setup.bash \
+  && catkin init \
+  && catkin build
+
+# Download srl-mav-sim
+RUN mkdir $HOME/.ssh/
+RUN echo "$SSH_PRIV_KEY" > $HOME/.ssh/id_rsa
+RUN chmod 600 $HOME/.ssh/id_rsa
+RUN touch $HOME/.ssh/known_hosts
+RUN ssh-keyscan github.com >> $HOME/.ssh/known_hosts
+RUN ssh-keyscan bitbucket.org >> $HOME/.ssh/known_hosts
+WORKDIR $HOME/catkin_ws/src
+RUN git clone --recurse-submodules git@github.com:smartroboticslab/srl-mav-sim.git
+RUN git clone --recurse-submodules git@github.com:smartroboticslab/mpc_ros.git
+RUN git clone git@github.com:smartroboticslab/mav_ekf.git
+RUN rm -rf $HOME/.ssh/id_rsa
+
+# Install srl-mav-sim
+WORKDIR $HOME/catkin_ws/src
+RUN pip3 install kconfiglib
+USER root
+RUN apt-get install ant openjdk-11-jdk -qy
+RUN ./srl-mav-sim/PX4-Autopilot/Tools/setup/ubuntu.sh --no-nuttx --no-sim-tools
+USER ${USER}
+
+# Install GeographicLib datasets
+WORKDIR $HOME/catkin_ws
+USER root
+RUN wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh
+RUN chmod +x install_geographiclib_datasets.sh
+RUN sudo bash ./install_geographiclib_datasets.sh
+RUN rm install_geographiclib_datasets.sh
+USER ${USER}
+
+# Build catkin workspace
+WORKDIR $HOME/catkin_ws
+USER root
+RUN apt-get install \
+  libgflags-dev \
+  libgoogle-glog-dev \
+  ros-${ROS_DISTRO}-gazebo-msgs \
+  libignition-gazebo3-* \
+  libignition-transport8-* \
+  -qy
+USER ${USER}
+RUN env IGNITION_VERSION=fortress catkin build -DCMAKE_BUILD_TYPE=Release
+
+# Install QGroundControl
+USER root
+RUN usermod -a -G dialout $USER
+RUN apt-get remove modemmanager -y
+RUN apt install gstreamer1.0-plugins-bad gstreamer1.0-libav gstreamer1.0-gl -y
+RUN apt install fuse libfuse2 -y
+RUN apt install libxcb-xinerama0 libxkbcommon-x11-0 libxcb-cursor0 -y
+USER ${USER}
+WORKDIR $HOME
+RUN wget https://d176tv9ibo4jno.cloudfront.net/latest/QGroundControl.AppImage
+RUN chmod +x QGroundControl.AppImage
+
+# Tools
+USER root
+WORKDIR $HOME
+# -- Install Neovim
+RUN curl -LO https://github.com/neovim/neovim/releases/latest/download/nvim-linux64.tar.gz \
+  && sudo rm -rf /opt/nvim \
+  && sudo tar -C /opt -xzf nvim-linux64.tar.gz \
+  && rm nvim-linux64.tar.gz \
+  && echo 'export PATH=$PATH:/opt/nvim-linux64/bin' > $HOME/.bashrc
+# -- Install vim and vifm
+RUN apt-get install -qy vim vifm
+# -- Add sane tmux configs
+RUN echo "set -g default-command /bin/bash" >> $HOME/.tmux.conf
+RUN echo "set -g mouse on" >> $HOME/.tmux.conf
+RUN echo "bind-key h select-pane -L" >> $HOME/.tmux.conf
+RUN echo "bind-key j select-pane -D" >> $HOME/.tmux.conf
+RUN echo "bind-key k select-pane -U" >> $HOME/.tmux.conf
+RUN echo "bind-key l select-pane -R" >> $HOME/.tmux.conf
+USER ${USER}
+
+
+# Entry point script
+WORKDIR $HOME
+COPY entry.sh /
+ENTRYPOINT ["/entry.sh"]
diff --git a/docker/build_docker_image.bash b/docker/build_docker_image.bash
new file mode 100755
index 0000000..11b0ce8
--- /dev/null
+++ b/docker/build_docker_image.bash
@@ -0,0 +1,3 @@
+#!/bin/bash
+set -e
+docker build -t srl_mav_sim --build-arg SSH_PRIV_KEY="$(cat ~/.ssh/id_rsa)" .
diff --git a/docker/entry.sh b/docker/entry.sh
new file mode 100755
index 0000000..2046e5c
--- /dev/null
+++ b/docker/entry.sh
@@ -0,0 +1,8 @@
+#!/bin/bash
+set -e
+source /opt/ros/noetic/setup.bash
+source $HOME/catkin_ws/devel/setup.bash
+export IGNITION_VERSION=fortress
+
+# Start an interactive shell
+/bin/bash
diff --git a/scripts/install_docker.bash b/scripts/install_docker.bash
new file mode 100755
index 0000000..0708c99
--- /dev/null
+++ b/scripts/install_docker.bash
@@ -0,0 +1,28 @@
+#!/bin/bash
+set -e
+
+# Install deps
+sudo apt-get update -y -qq
+sudo apt-get install -y -qq \
+	ca-certificates \
+  curl \
+  gnupg \
+  lsb-release
+
+# Install GPG key and docker sources
+sudo mkdir -p /etc/apt/keyrings
+curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo gpg --dearmor -o /etc/apt/keyrings/docker.gpg
+echo "deb [arch=$(dpkg --print-architecture) signed-by=/etc/apt/keyrings/docker.gpg] https://download.docker.com/linux/ubuntu $(lsb_release -cs) stable" | sudo tee /etc/apt/sources.list.d/docker.list > /dev/null
+
+# Install Docker
+sudo apt-get update -y -qq
+sudo apt-get install -q -yy \
+  docker-ce \
+  docker-ce-cli \
+  containerd.io \
+  docker-compose-plugin
+
+# Create and add user to docker group
+groupadd -f docker
+sudo usermod -aG docker ${USER}
+echo "You now need to log out and log back in so that your group membership is re-evaluated"
diff --git a/scripts/test_offboard_mode.py b/scripts/test_offboard_mode.py
new file mode 100755
index 0000000..03df289
--- /dev/null
+++ b/scripts/test_offboard_mode.py
@@ -0,0 +1,62 @@
+#!/usr/bin/env python3
+import time
+
+import rospy
+from geometry_msgs.msg import PoseStamped
+from mavros_msgs.msg import State
+from mavros_msgs.srv import CommandBool, CommandBoolRequest, SetMode, SetModeRequest
+
+current_state = State()
+
+def state_cb(msg):
+    global current_state
+    current_state = msg
+
+
+if __name__ == "__main__":
+    rospy.init_node("test_offboard_mode")
+    state_sub = rospy.Subscriber("mavros/state", State, callback = state_cb)
+    local_pos_pub = rospy.Publisher("/mavros/setpoint_position/local", PoseStamped, queue_size=10)
+    rospy.wait_for_service("/mavros/cmd/arming")
+    arming_client = rospy.ServiceProxy("mavros/cmd/arming", CommandBool)
+    rospy.wait_for_service("/mavros/set_mode")
+    set_mode_client = rospy.ServiceProxy("mavros/set_mode", SetMode)
+
+    # Setpoint publishing MUST be faster than 2Hz
+    rate = rospy.Rate(20)
+
+    # Wait for Flight Controller connection
+    while(not rospy.is_shutdown() and not current_state.connected):
+        rate.sleep()
+    rospy.loginfo("Connected!")
+
+    # Send a few setpoints before starting
+    pose = PoseStamped()
+    pose.pose.position.x = 0
+    pose.pose.position.y = 0
+    pose.pose.position.z = 2
+    for i in range(100):
+        if(rospy.is_shutdown()):
+            break
+        local_pos_pub.publish(pose)
+        rate.sleep()
+
+    # Offboard mode
+    offb_set_mode = SetModeRequest()
+    offb_set_mode.custom_mode = 'OFFBOARD'
+    set_mode_client.call(offb_set_mode)
+    rospy.loginfo("Sent offboard mode!")
+    time.sleep(5)
+
+    # Arm Command
+    arm_cmd = CommandBoolRequest()
+    arm_cmd.value = True
+    arming_client.call(arm_cmd)
+    rospy.loginfo("Sent ARM command!")
+    rate.sleep()
+
+    # Keep publishing pose setpoint
+    rospy.loginfo("Sending position setpoint!")
+    while(not rospy.is_shutdown()):
+        local_pos_pub.publish(pose)
+        rate.sleep()