diff --git a/conf/airframes/ENAC/quadrotor/anton_indi_aruco.xml b/conf/airframes/ENAC/quadrotor/anton_indi_aruco.xml
index 6a496b44b9..7c2b17d61f 100644
--- a/conf/airframes/ENAC/quadrotor/anton_indi_aruco.xml
+++ b/conf/airframes/ENAC/quadrotor/anton_indi_aruco.xml
@@ -236,14 +236,13 @@
   <section name="TAG_TRACKING" prefix="TAG_TRACKING_">
     <define name="BODY_TO_CAM_PSI" value="0"/>
     <define name="CAM_POS_Y" value="-0.12"/>
-    <define name="NB_WP_MAX" value="2"/>
     <define name="WPS" type="array">
       <field type="struct">
         <field name="wp_id" value="WP_TAG"/>
         <field name="tag_id" value="0"/>
       </field>
       <field type="struct">
-        <field name="wp_id" value="WP_ROVER"/>
+        <field name="wp_id" value="WP_TRUCK"/>
         <field name="tag_id" value="4"/>
       </field>
     </define>
diff --git a/conf/flight_plans/ENAC/demo_aruco_drop_indoor.xml b/conf/flight_plans/ENAC/demo_aruco_drop_indoor.xml
index 8d69300690..84719a41f5 100644
--- a/conf/flight_plans/ENAC/demo_aruco_drop_indoor.xml
+++ b/conf/flight_plans/ENAC/demo_aruco_drop_indoor.xml
@@ -104,7 +104,10 @@
       <call_once fun="jevois_stream(false)"/>
       <stay wp="STDBY"/>
     </block>
-
+    <block name="Stay Tag" strip_button="Stay Tag" group="tag">
+      <call_once fun="jevois_stream(true)"/>
+      <stay wp="TAG" height="search_height" pre_call="nav_set_heading_deg(tag_tracking_get_heading(11))"/>
+    </block>
     <block name="Search Tag" strip_button="Search Target" group="tag">
       <call_once fun="jevois_stream(true)"/>
       <call_once fun="nav_survey_rectangle_rotorcraft_setup(WP_S1, WP_S3, 1, NS)"/>
@@ -115,7 +118,6 @@
       <exception cond="rectangle_survey_sweep_num == 1" deroute="Standby"/>
       <call fun="nav_survey_rectangle_rotorcraft_run(WP_S1, WP_S3)"/>
     </block>
-
     <block name="Follow Tag" strip_button="Follow Tag" group="tag">
       <call_once fun="jevois_stream(true)"/>
       <set var="tag_tracking_get(0)->motion_type" value="TAG_TRACKING_MOVING"/>
diff --git a/conf/flight_plans/competitions/IMAV2022_drop.xml b/conf/flight_plans/competitions/IMAV2022_drop.xml
index 4c2f0d486d..3395b4d248 100644
--- a/conf/flight_plans/competitions/IMAV2022_drop.xml
+++ b/conf/flight_plans/competitions/IMAV2022_drop.xml
@@ -273,7 +273,7 @@
       <call_once fun="guidance_h_SetMaxSpeed(search_speed)"/>
       <call_once fun="NavSetWaypointHere(WP_TAG)"/>
       <call_once fun="jevois_stream(true)"/>
-      <stay wp="DELIVERY" until="(tag_tracking.status == TAG_TRACKING_RUNNING) @OR (stage_time @GT 15)" height="search_height"/>
+      <stay wp="DELIVERY" until="(tag_tracking_get(TAG_TRACKING_ANY) @DEREF status == TAG_TRACKING_RUNNING) @OR (stage_time @GT 15)" height="search_height"/>
       <deroute block="Drop Package"/>
     </block>
 
@@ -287,7 +287,7 @@
       <call_once fun="guidance_h_SetMaxSpeed(search_speed)"/>
       <call_once fun="NavSetWaypointHere(WP_TAG)"/>
       <call_once fun="jevois_stream(true)"/>
-      <stay wp="SILENT" until="(tag_tracking.status == TAG_TRACKING_RUNNING) @OR (stage_time @GT 15)" height="search_height"/>
+      <stay wp="SILENT" until="(tag_tracking_get(TAG_TRACKING_ANY) @DEREF status == TAG_TRACKING_RUNNING) @OR (stage_time @GT 15)" height="search_height"/>
       <deroute block="Drop Package"/>
     </block>
 
@@ -315,7 +315,7 @@
       <call_once fun="guidance_h_SetMaxSpeed(search_speed)"/>
       <call_once fun="NavSetWaypointHere(WP_TAG)"/>
       <call_once fun="jevois_stream(true)"/>
-      <stay wp="UNCERTAIN" until="(tag_tracking.status == TAG_TRACKING_RUNNING) @OR (stage_time @GT 15)" height="search_height"/>
+      <stay wp="UNCERTAIN" until="(tag_tracking_get(TAG_TRACKING_ANY) @DEREF status == TAG_TRACKING_RUNNING) @OR (stage_time @GT 15)" height="search_height"/>
       <call_once fun="NavSetWaypointHere(WP_UNCERTAIN)"/>
       <deroute block="Drop Package"/>
     </block>
@@ -330,13 +330,13 @@
       <call_once fun="guidance_h_SetMaxSpeed(search_speed)"/>
       <call_once fun="NavSetWaypointHere(WP_TAG)"/>
       <call_once fun="jevois_stream(true)"/>
-      <stay wp="AREA_TAG" until="(tag_tracking.status == TAG_TRACKING_RUNNING) @OR (stage_time @GT 15)" height="search_height"/>
+      <stay wp="AREA_TAG" until="(tag_tracking_get(TAG_TRACKING_ANY) @DEREF status == TAG_TRACKING_RUNNING) @OR (stage_time @GT 15)" height="search_height"/>
       <deroute block="Drop Package"/>
     </block>
 
     <block name="Goto Truck">
       <set var="mission_nb" value="DROP_NONE"/>
-      <set var="tag_tracking.motion_type" value="TAG_TRACKING_MOVING"/>
+      <set var="tag_tracking_get(TAG_TRACKING_ANY)->motion_type" value="TAG_TRACKING_MOVING"/>
       <call_once fun="NavSetWaypointHere(WP__HERE)"/>
       <call_once fun="jevois_stream(false)"/>
       <!--call_once fun="guidance_h_SetMaxSpeed(goto_speed)"/-->
@@ -348,13 +348,13 @@
       <call_once fun="guidance_h_SetMaxSpeed(search_speed)"/>
       <call_once fun="NavSetWaypointHere(WP_TAG)"/>
       <stay wp="TRUCK" until="stage_time @GT 5" height="truck_height"/>
-      <stay wp="TRUCK" until="(tag_tracking.status == TAG_TRACKING_RUNNING) @OR (stage_time @GT 15)" height="truck_height"/>
+      <stay wp="TRUCK" until="(tag_tracking_get(TAG_TRACKING_ANY) @DEREF status == TAG_TRACKING_RUNNING) @OR (stage_time @GT 15)" height="truck_height"/>
       <deroute block="Drop Package on Truck"/>
     </block>
 
     <block name="Run Search Uncertain">
       <set var="mission_nb" value="DROP_NONE"/>
-      <exception cond="tag_tracking.status == TAG_TRACKING_RUNNING" deroute="Drop Package"/>
+      <exception cond="tag_tracking_get(TAG_TRACKING_ANY) @DEREF status == TAG_TRACKING_RUNNING" deroute="Drop Package"/>
       <call_once fun="NavSetWaypointHere(WP__HERE)"/>
       <call_once fun="jevois_stream(false)"/>
       <call_once fun="guidance_h_SetMaxSpeed(goto_speed)"/>
@@ -387,8 +387,8 @@
 
     <block name="Run Search Truck">
       <set var="mission_nb" value="DROP_NONE"/>
-      <exception cond="tag_tracking.status == TAG_TRACKING_RUNNING" deroute="Drop Package on Truck"/>
-      <set var="tag_tracking.motion_type" value="TAG_TRACKING_MOVING"/>
+      <exception cond="tag_tracking_get(TAG_TRACKING_ANY) @DEREF status == TAG_TRACKING_RUNNING" deroute="Drop Package on Truck"/>
+      <set var="tag_tracking_get(TAG_TRACKING_ANY)->motion_type" value="TAG_TRACKING_MOVING"/>
       <call_once fun="NavSetWaypointHere(WP__HERE)"/>
       <call_once fun="jevois_stream(false)"/>
       <call_once fun="guidance_h_SetMaxSpeed(goto_speed)"/>
@@ -418,21 +418,21 @@
     </block>
 
     <block name="Drop Package">
-      <stay wp="TAG" until="(stage_time @GT 2 @AND tag_tracking.status == TAG_TRACKING_RUNNING) @OR (stage_time @GT 5)" height="search_height"/>
+      <stay wp="TAG" until="(stage_time @GT 2 @AND tag_tracking_get(TAG_TRACKING_ANY) @DEREF status == TAG_TRACKING_RUNNING) @OR (stage_time @GT 5)" height="search_height"/>
       <stay wp="TAG" climb="-0.5" vmode="climb" until="((tag_distance @LT drop_height) @AND tag_valid) @OR (GetPosHeight() @LT drop_height/2.)" post_call="tag_valid = false"/>
       <call_once fun="DropOpen()"/>
       <stay wp="TAG" until="stage_time @GT 3" height="goto_height"/>
-      <set var="tag_tracking.motion_type" value="TAG_TRACKING_FIXED_POS"/>
+      <set var="tag_tracking_get(TAG_TRACKING_ANY)->motion_type" value="TAG_TRACKING_FIXED_POS"/>
       <deroute block="Land"/>
     </block>
 
     <block name="Drop Package on Truck">
-      <exception cond="tag_tracking.status == TAG_TRACKING_SEARCHING @OR tag_tracking.status == TAG_TRACKING_LOST" deroute="Drop on Truck fallback"/>
+      <exception cond="tag_tracking_get(TAG_TRACKING_ANY) @DEREF status == TAG_TRACKING_SEARCHING @OR tag_tracking_get(TAG_TRACKING_ANY) @DEREF status == TAG_TRACKING_LOST" deroute="Drop on Truck fallback"/>
       <stay wp="TAG" until="stage_time @GT 3" height="truck_height"/>
       <stay wp="TAG" climb="-0.5" vmode="climb" until="((tag_distance @LT drop_height+0.5) @AND tag_valid) @OR (GetPosHeight() @LT drop_height+1.)" post_call="tag_valid = false"/>
       <call_once fun="DropOpen()"/>
       <stay wp="TAG" until="stage_time @GT 3" height="goto_height"/>
-      <set var="tag_tracking.motion_type" value="TAG_TRACKING_FIXED_POS"/>
+      <set var="tag_tracking_get(TAG_TRACKING_ANY)->motion_type" value="TAG_TRACKING_FIXED_POS"/>
       <set var="follow_follow_wp_status" value="MODULES_STOP"/>
       <deroute block="Land"/>
     </block>
@@ -442,7 +442,7 @@
       <stay wp="TRUCK" climb="-0.5" vmode="climb" until="GetPosHeight() @LT drop_height+1.0"/>
       <call_once fun="DropOpen()"/>
       <stay wp="TRUCK" until="stage_time @GT 3" height="goto_height"/>
-      <set var="tag_tracking.motion_type" value="TAG_TRACKING_FIXED_POS"/>
+      <set var="tag_tracking_get(TAG_TRACKING_ANY)->motion_type" value="TAG_TRACKING_FIXED_POS"/>
       <set var="follow_follow_wp_status" value="MODULES_STOP"/>
       <deroute block="Land"/>
     </block>
diff --git a/conf/modules/tag_tracking.xml b/conf/modules/tag_tracking.xml
index 0a6c7918b0..599ab3ed55 100644
--- a/conf/modules/tag_tracking.xml
+++ b/conf/modules/tag_tracking.xml
@@ -2,11 +2,11 @@
 <module name="tag_tracking" dir="computer_vision">
   <doc>
     <description>
-      Track poistion of a tag (ArUco, QRcode, ...) detected by an onboard camera
+      Track position of a tag (ArUco, QRcode, ...) detected by an onboard camera
       The tag detection and pose computation is done outside of the module, only the estimation by fusion of AHRS and visual detection with a Kalman filter is performed in this module
     </description>
     <section name="TAG_TRACKING" prefix="TAG_TRACKING_">
-      <define name="WP" value="WP_TAG" description="if defined, update the position of the waypoint at the location of the tag; if TAG waypoint exists in flight plan, it is used by default"/>
+      <define name="WPS" value="[{.wp_id=WP_TAG, .tag_id=42}, ...]" description="Array of structs. If defined, update the position of the waypoints at the location of the tags"/>
       <define name="SIM_WP" value="WP_TARGET" description="designate a waypoint to be used as virtual tag in simulation; if TARGET waypoint exists in flight plan, it is used by default"/>
       <define name="BODY_TO_CAM_PHI" value="0." description="rotation from body to cam frame (phi)"/>
       <define name="BODY_TO_CAM_THETA" value="0." description="rotation from body to cam frame (theta)"/>
diff --git a/sw/airborne/modules/computer_vision/tag_tracking.c b/sw/airborne/modules/computer_vision/tag_tracking.c
index 0b81700633..19735d47a3 100644
--- a/sw/airborne/modules/computer_vision/tag_tracking.c
+++ b/sw/airborne/modules/computer_vision/tag_tracking.c
@@ -148,9 +148,6 @@ float speed_circle = 0.03;
 #define TAG_TRACKING_MAX_VZ 2.f
 #endif
 
-#ifndef TAG_TRACKING_NB_WP_MAX
-#define TAG_TRACKING_NB_WP_MAX 1
-#endif
 
 #define TAG_UNUSED_ID -1
 
@@ -160,8 +157,10 @@ static const float tag_track_dt = TAG_TRACKING_PROPAGATE_PERIOD;
 // global state structure
 struct tag_tracking {
   struct FloatVect3 meas;       ///< measured position
+  struct FloatQuat cam_to_tag_quat;   ///< measured quat
 
   struct FloatRMat body_to_cam; ///< Body to camera rotation
+  struct FloatQuat body_to_cam_quat;  ///< Body to camera rotation in quaternion
   struct FloatVect3 cam_pos;    ///< Position of camera in body frame
 
   float timeout;                ///< timeout for lost flag [sec]
@@ -186,10 +185,11 @@ struct wp_tracking {
 
 
 #if (defined TAG_TRACKING_WPS)
-struct wp_tracking wp_track[TAG_TRACKING_NB_WP_MAX] = TAG_TRACKING_WPS;
+struct wp_tracking wp_track[] = TAG_TRACKING_WPS;
+const uint8_t tag_tracking_wps_len = sizeof(wp_track) / sizeof(struct wp_tracking);
 #else
-#error "TAG_TRACKING_NB_WP_MAX not defined!"
-struct wp_tracking wp_track[TAG_TRACKING_NB_WP_MAX] = {0};
+struct wp_tracking wp_track[] = {};
+const uint8_t tag_tracking_wps_len = 0;
 #endif
 
 
@@ -197,6 +197,8 @@ struct tag_info tag_infos[TAG_TRACKING_NB_MAX];
 
 struct tag_tracking_public dummy = {0};
 
+struct FloatQuat rot_x_quat;  // quaternion to rotate tag to have Z down
+
 // Abi bindings
 #ifndef TAG_TRACKING_ID
 #define TAG_TRACKING_ID ABI_BROADCAST
@@ -206,22 +208,30 @@ static abi_event tag_track_ev;
 
 static void tag_tracking_propagate_start_tag(struct tag_info* tag_info);
 
-struct tag_tracking_public* tag_tracking_get(uint8_t tag_id) {
+struct tag_tracking_public* tag_tracking_get(int16_t tag_id) {
   for(int i=0; i<TAG_TRACKING_NB_MAX; i++) {
     if(tag_infos[i].tag_track_private.id == tag_id) {
       return &tag_infos[i].tag_tracking;
     }
 
-    // tag_id == 0 returns the first running tag.
-    if(tag_id == 0 && tag_infos[i].tag_tracking.status == TAG_TRACKING_RUNNING) {
+    // tag_id == TAG_TRACKING_ANY, returns the first running tag.
+    if(tag_id == TAG_TRACKING_ANY && tag_infos[i].tag_tracking.status == TAG_TRACKING_RUNNING) {
       return &tag_infos[i].tag_tracking;
     }
   }
 
   dummy.status = TAG_TRACKING_SEARCHING;
+  float_quat_identity(&dummy.ned_to_tag_quat);
   return &dummy;
 }
 
+float tag_tracking_get_heading(int16_t tag_id) {
+  struct tag_tracking_public* tag = tag_tracking_get(tag_id);
+  struct FloatEulers e;
+  float_eulers_of_quat(&e, &tag->ned_to_tag_quat);
+  return DegOfRad(e.psi);
+}
+
 // update measure vect before calling
 static void update_tag_position(struct tag_info* tag_info)
 {
@@ -236,6 +246,10 @@ static void update_tag_position(struct tag_info* tag_info)
   struct NedCoor_f * pos_ned = stateGetPositionNed_f();
   VECT3_ADD(target_pos_ned, *pos_ned);
 
+  // TODO filter in kalman ?
+  float_quat_comp(&tag_info->tag_tracking.body_to_tag_quat, &tag_infos->tag_track_private.body_to_cam_quat, &tag_info->tag_track_private.cam_to_tag_quat);
+  float_quat_comp(&tag_info->tag_tracking.ned_to_tag_quat, stateGetNedToBodyQuat_f(), &tag_info->tag_tracking.body_to_tag_quat);
+
   if (tag_info->tag_tracking.status == TAG_TRACKING_DISABLE) {
     // don't run kalman, just update pos, set speed to zero
     tag_info->tag_tracking.pos = target_pos_ned;
@@ -265,23 +279,27 @@ static void tag_track_cb(uint8_t sender_id UNUSED,
   if (type == JEVOIS_MSG_D3) {
     int16_t tag_id = (int16_t)jevois_extract_nb(id);
     for(int i=0; i<TAG_TRACKING_NB_MAX; i++) {
-      if(tag_infos[i].tag_track_private.id != tag_id && tag_infos[i].tag_track_private.id != TAG_UNUSED_ID) {
-        continue;
-      }
+      // free slot, store tag ID
       if(tag_infos[i].tag_track_private.id == TAG_UNUSED_ID) {
-        // store tag ID
         tag_infos[i].tag_track_private.id = tag_id;
       }
-      // store data from Jevois detection
-      tag_infos[i].tag_track_private.meas.x = coord[0] * TAG_TRACKING_COORD_TO_M;
-      tag_infos[i].tag_track_private.meas.y = coord[1] * TAG_TRACKING_COORD_TO_M;
-      tag_infos[i].tag_track_private.meas.z = coord[2] * TAG_TRACKING_COORD_TO_M;
-      // update filter
-      update_tag_position(&tag_infos[i]);
-      // reset timeout and status
-      tag_infos[i].tag_track_private.timeout = 0.f;
-      tag_infos[i].tag_track_private.updated = true;
-      break;
+
+      if(tag_infos[i].tag_track_private.id == tag_id) {
+        // store data from Jevois detection
+        tag_infos[i].tag_track_private.meas.x = coord[0] * TAG_TRACKING_COORD_TO_M;
+        tag_infos[i].tag_track_private.meas.y = coord[1] * TAG_TRACKING_COORD_TO_M;
+        tag_infos[i].tag_track_private.meas.z = coord[2] * TAG_TRACKING_COORD_TO_M;
+
+        float_quat_normalize(&quat);
+        // rotate the quaternion so Z is down
+        float_quat_comp(&tag_infos[i].tag_track_private.cam_to_tag_quat, &quat, &rot_x_quat);
+        // update filter
+        update_tag_position(&tag_infos[i]);
+        // reset timeout and status
+        tag_infos[i].tag_track_private.timeout = 0.f;
+        tag_infos[i].tag_track_private.updated = true;
+        break;
+      }
     }
   }
 }
@@ -304,7 +322,7 @@ void tag_tracking_parse_target_pos(uint8_t *buf)
 }
 
 // Update and display tracking WP
-static void update_wp(struct tag_info* tag_info, bool report UNUSED)
+static void update_wp(struct tag_info* tag_info UNUSED, bool report UNUSED)
 {
 #ifdef TAG_TRACKING_WPS
 
@@ -337,18 +355,23 @@ static void update_wp(struct tag_info* tag_info, bool report UNUSED)
 // Init function
 void tag_tracking_init()
 {
+  struct FloatEulers rot_x = { M_PI, 0, 0};
+  float_quat_of_eulers(&rot_x_quat, &rot_x);
+
   // Init structure
   for(int i=0; i<TAG_TRACKING_NB_MAX; i++) {
     FLOAT_VECT3_ZERO(tag_infos[i].tag_track_private.meas);
     FLOAT_VECT3_ZERO(tag_infos[i].tag_tracking.pos);
     FLOAT_VECT3_ZERO(tag_infos[i].tag_tracking.speed);
     FLOAT_VECT3_ZERO(tag_infos[i].tag_tracking.speed_cmd);
+    float_quat_identity(&tag_infos[i].tag_tracking.ned_to_tag_quat);
     struct FloatEulers euler = {
       TAG_TRACKING_BODY_TO_CAM_PHI,
       TAG_TRACKING_BODY_TO_CAM_THETA,
       TAG_TRACKING_BODY_TO_CAM_PSI
     };
     float_rmat_of_eulers(&tag_infos[i].tag_track_private.body_to_cam, &euler);
+    float_quat_of_eulers(&tag_infos[i].tag_track_private.body_to_cam_quat, &euler);
     VECT3_ASSIGN(tag_infos[i].tag_track_private.cam_pos,
         TAG_TRACKING_CAM_POS_X,
         TAG_TRACKING_CAM_POS_Y,
@@ -365,7 +388,7 @@ void tag_tracking_init()
   }
 
   // reserve slots for tag_ids we are looking for, and associate wp_ids.
-  for(int i=0; i<Min(TAG_TRACKING_NB_WP_MAX, TAG_TRACKING_NB_MAX); i++) {
+  for(int i=0; i<Min(tag_tracking_wps_len, TAG_TRACKING_NB_MAX); i++) {
     tag_infos[i].tag_track_private.id = wp_track[i].tag_id;
     tag_infos[i].wp_id = wp_track[i].wp_id;
   }
diff --git a/sw/airborne/modules/computer_vision/tag_tracking.h b/sw/airborne/modules/computer_vision/tag_tracking.h
index 2355b7ecb1..8abc91723a 100644
--- a/sw/airborne/modules/computer_vision/tag_tracking.h
+++ b/sw/airborne/modules/computer_vision/tag_tracking.h
@@ -38,7 +38,7 @@
 #endif
 
 
-
+#define TAG_TRACKING_ANY -2
 
 // Searching status
 #define TAG_TRACKING_SEARCHING  0
@@ -52,14 +52,16 @@
 #define TAG_TRACKING_MOVING     1
 
 struct tag_tracking_public {
-  struct FloatVect3 pos;        ///< estimated position
-  struct FloatVect3 speed;      ///< estimated speed
-  uint8_t status;               ///< tracking status flag
-  uint8_t motion_type;          ///< type of tag motion
-  float predict_time;           ///< prediction time for WP tag
-  struct NedCoor_f speed_cmd;   ///< speed command to track the tag position
-  float kp;                     ///< horizontal tracking command gain
-  float kpz;                    ///< vertical tracking command gain
+  struct FloatVect3 pos;            ///< estimated position
+  struct FloatVect3 speed;          ///< estimated speed
+  struct FloatQuat body_to_tag_quat;///< estimated attitude in body frame
+  struct FloatQuat ned_to_tag_quat; ///< estimated attitude in NED frame
+  uint8_t status;                   ///< tracking status flag
+  uint8_t motion_type;              ///< type of tag motion
+  float predict_time;               ///< prediction time for WP tag
+  struct NedCoor_f speed_cmd;       ///< speed command to track the tag position
+  float kp;                         ///< horizontal tracking command gain
+  float kpz;                        ///< vertical tracking command gain
 };
 
 
@@ -75,13 +77,14 @@ void tag_tracking_set_predict_time(float predict_time);
 void tag_tracking_set_kp(float kp);
 void tag_tracking_set_kpz(float kpz);
 
-extern struct tag_tracking_public* tag_tracking_get(uint8_t tag_id);
+extern struct tag_tracking_public* tag_tracking_get(int16_t tag_id);
 extern void tag_tracking_init(void);
 extern void tag_tracking_propagate(void);
 extern void tag_tracking_propagate_start(void);
 extern void tag_tracking_report(void);
 extern void tag_tracking_parse_target_pos(uint8_t *buf);
 extern void tag_tracking_compute_speed(void);
+float tag_tracking_get_heading(int16_t tag_id);
 
 #endif  // TAG_TRACKING_H