Navigation & EKF¶

This section covers navigation systems and Extended Kalman Filter implementations.

AP_NavEKF (Navigation EKF)¶

Header: libraries/AP_NavEKF/AP_NavEKF.h

Description: Extended Kalman Filter for navigation. Provides position, velocity, and attitude estimates by fusing data from GPS, barometer, compass, and optical flow sensors.

EKF Versions:

EKF2 - Legacy EKF
EKF3 - Current EKF with improved features

Key Features:

Multi-sensor fusion
Multiple EKF lanes
IMU selection
Optical flow support
GPS yaw fusion
Vision position fusion

Usage Example:

#include <AP_NavEKF.h>

AP_NavEKF *ekf = AP_NavEKF::get_singleton();

// Check EKF health
if (ekf->healthy()) {
    // Get position
    Vector3f pos;
    ekf->get_relative_position_NED(pos);

    // Get velocity
    Vector3f vel;
    ekf->get_velocity_NED(vel);

    // Get attitude quaternion
    Quaternion quat;
    ekf->getQuaternion(quat);
}

EKF Source Selection ¶

AP_NavEKF_Source (EKF Position/Velocity Sources)¶

Header: libraries/AP_NavEKF2/AP_NavEKF_Source.h

Description: Manages EKF position and velocity source selection.

Sources:

GPS - GPS position
Optical Flow - Optical flow
Vision - Computer vision
Beacon - Beacon positioning

Usage Example:

AP_NavEKF_Source *ekf_source = AP_NavEKF_Source::get_singleton();

// Set position source
ekf_source->set_positype(AP_NavEKF_Source::SourceXY::GPS);

// Set velocity source
ekf_source->set_veltype(AP_NavEKF_Source::SourceXY::GPS);

Terrain System ¶

AP_Terrain (Terrain Database)¶

Header: libraries/AP_Terrain/AP_Terrain.h

Description: Terrain database system using SRTM or other terrain data.

Key Features:

SRTM terrain data
Grid-based terrain
Altitude correction
Terrain following

Usage Example:

#include <AP_Terrain.h>

AP_Terrain *terrain = AP_Terrain::get_singleton();

// Check if terrain data available
if (terrain->enabled() && terrain->healthy()) {
    // Get terrain height at location
    float terrain_height;
    if (terrain->get_height_amsl(location, terrain_height)) {
        // Use terrain height
        Serial.printf("Terrain height: %.1f m\n", terrain_height);
    }

    // Get altitude above terrain
    float alt_above_terrain;
    if (terrain->height_above_terrain(alt_above_terrain)) {
        Serial.printf("Above terrain: %.1f m\n", alt_above_terrain);
    }
}

Optical Flow ¶

AP_OpticalFlow (Optical Flow Sensor)¶

Header: libraries/AP_OpticalFlow/AP_OpticalFlow.h

Description: Optical flow sensor for indoor navigation and position estimation without GPS.

Supported Sensors:

PX4Flow
CFersky Flow
Digital Flow

Usage Example:

#include <AP_OpticalFlow.h>

AP_OpticalFlow *optical_flow = AP_OpticalFlow::get_singleton();

// Read sensor
optical_flow->update();

// Get flow
Vector2f flow;
optical_flow->flow_xy(flow);

// Get quality
uint8_t quality = optical_flow->quality();

// Get ground distance
float ground_height = optical_flow->ground_height();

AP_Beacon (Beacon Positioning)¶

Header: libraries/AP_Beacon/AP_Beacon.h

Description: Beacon positioning system for indoor/local navigation.

Usage Example:

#include <AP_Beacon.h>

AP_Beacon *beacon = AP_Beacon::get_singleton();

// Get position
Vector3f position;
if (beacon->get_position(position)) {
    Serial.printf("Position: %.2f, %.2f, %.2f\n",
                position.x, position.y, position.z);
}