I have a question ，in feature_match_ardu.py :print("Offset x, y(in cms): ",-self.inCMY, self.inCMX)
Offset x,y(in cms) is for the picture(satellite image)?or something else ?
Hi @JR_C ! The offset here are the estimated positing of the drone on the satellite image. “in cms” is a typo here and that’s actually in meters. If x and y equals 20m and 30m, that’s from the takeoff position. In short, these offsets are actual distance between origin and the current position(as viewed and calculated using camera frame and pixel distance).
I found that you should set the param SR1_POSITION at least more than 1, so the AP will send the GLOBAL_POSITION_INT mavlink message
You can add a param( SR1_POSITION 1) to the file azebo-iris-gimbal-ngps.parm
1 Like
Do you have any progress in the part down below?:
Location interpolation based on GPS EXIF data
1 Like
Hi @JR_C ! Yes! That is done but I guess it’s private. Let me make it public and you can find that in same repo
Thanks
dear snktshrma, it is really a great work.
i wanted to see in the source code what you did to make the image suitable for matching like color corrections, geometric corrections etc.., but i did not found the solution.
could you tell where i can find it in the source code.
Update: Transformer & Optimization–Based NGPS Fusion
1. SIFT-Based Prototype
In my original GSOC post, I described a proof-of-concept NGPS pipeline that relied on SIFT feature matching against a stitched map. While functional, it generally fails under varying lighting and texture conditions, requires existing database, and its update rate oscillated between 0.5 Hz and 3 Hz.
2. Transformer-Based VPS & VIO
To improve, I replaced SIFT with a learned matching approach. I extract sparse features using SuperPoint, then feed them into a lightweight transformer model that dynamically prunes and matches descriptors at inference, achieving high rate output. I paired it with a VIO back-end. This provided local poses at 10 Hz and higher as configured and data inputs.
3. EKF Extension Attempt
Before moving to global optimization, I tried to extend the ArduPilot’s AP_NavEKF3 to fuse both VIO poses and coarse VPS estimates. I modified the EKF update step and made a drop-state measurement model, but the added covariance propagation and measurement updates increased processing and memory usage and triggered watchdog errors. I’m sure @rmackay9 sir might have an explanation for this. I suspect my own code changes introduced some inefficiencies.
In any case, I didn’t work on it further as I was eager to implement offboard estimation.
4. IMU + Optical-Flow + VPS: Optimization-Based Fusion
So finally I resorted to fusing all constraints in a two-stage optimization pipeline using the modified VINS algorithm: one for a local VIO step using IMU + Kanade-Lucas-Tomasi sparse optical flow and another global optimization step using VPS. The target 4DOF state vector (position, orientation(yaw), and velocity) is optimized over IMU preintegration residuals, optical-flow reprojection errors, and a global optimization problem in the second stage. The optimizer in a fixed-size sliding window provided estimates rates of ≥ 20 Hz on an i7 CPU and RTX 3060 GPU, and tested >10 Hz on Jetson Orin NX 8 GB (inference only).
Altitude and yaw are handled by barometer and compass estimates from the onboard EKF, respectively.
Estimated UAV trajectory at 150 m altitude overlaid on satellite imagery. Left: raw VPS. Middle: VPS + moving average. Right: VPS + Gaussian averaging.
Downward-looking camera footage (×4 speed) used for VPS map matching at 150 m altitude flight.
Click on this to redirect to the Raw VPS estimation inferencing visualization video
5. UKF Prototype vs. Direct Optimization
I also prototyped an Unscented Kalman Filter to fuse VIO and VPS. Although, as expected, the UKF provided pretty good estimates but I resorted to optimization based fusion as … I just wanted to do so : ) The measurement model I designed for UKF is a simple drop state model, considering the input is already compatible with states.
6. Testings and Development
All components run as ROS 2 nodes: fusion_node for VIO, vit_matcher_node for VPS, and ngps_fusion_node for the global optimizer. I resorted to use MAVROS initially as messaging protocol but in final version will shift to native AP DDS. I simulated everything on Gazebo Harmonic and ROS Humble.
Simulation results: ground truth vs. raw VPS vs. VPS fused with VIO.
A 3-point triangle trajectory simulated in gazebo
The ROS2 bag file for this tests is around 14GBs. I am still figuring out a way to share that;
7. Calibrations
Mainly for hardware, I measured IMU noise and bias and used Kalibr to find the camera-IMU time offset and extrinsics.
8. Hardware Trials
For hardware trials, thanks to @kkouer for helping me with actual flight tests to test the algorithm and providing the dataset for training and optimizations. We are still in process of conducting a safe online HITL tests.
Also @ppoirier has been a huge help all along : )
9. Code & Release Plan
I’m still refactoring code into clean, modular ROS 2 packages and solving some bugs along as I conduct tests. It’s a mess now because of experimentation and development. I’ll push the standalone UKF module along with it. All repositories will be open-sourced in the coming weeks after hardware tests are verified.
I have already open sourced my colab notebooks that contains crude implementation of the VPS algorithm. You can find it on my existing ap_ngps repo.
10. Future Plan
This project exceeded than originally planned timeline because I underestimated the work involved : )
I am planning to use a loop closure algorithm based on traditional descriptors and BoW in future with improved frame alignment and interpolated correction. The main reason for not using loop closure in current implementation is that at high altitudes, the accumulated drift can reach tens of meters compared to in centimeters in case of indoor positioning and estimation. The loop closure could cause large discontinuities in the AP’s EKF if fed directly and sure it won’t be good for the drone. To avoid those jumps, a simple transformation + interpolation, followed by a simple low pass filter might help in blending overtime.
Apart from this, I am confident that using SFM, we can build a full fledged SLAM with a larger baseline stereo for mapping purposes with higher precision.
Most of the existing solutions out there have some drift involved in their non-GPS positioning but I aim to nullify that and make it really robust. It still have a lot of work required to make it as I want but it’s really a learning roller coaster.
Thanks : )
5 Likes
This is an awesome achievement @snktshrma, congratulation for your commitment and determination to make possible usage of VPS as an alternative GPS denied navigation.
Bringing a new concept to life can be really challenging, especially when there aren’t many resources or references to draw from. That’s why I found your approach so inspiring—by refining things iteratively and building on an existing framework, you create a strong foundation that makes it much easier to gain momentum and see real progress
Keep on the good work !!
3 Likes
Thanks a lot @ppoirier! Means a lot : )
Hi everyone, thanks for all the feedback!
I’ve created up a new discussion thread for the newer updates to track progress and discussions.
Thanks : )
2 Likes