Odor Source Localization

Architectural Setup & Environment Initialization

#include <vector>
#include <cmath>
#include <Eigen/Dense>

class OdorGridMap {
public:
    OdorGridMap(int width, int height, double resolution)
        : width_(width), height_(height), resolution_(resolution) {
        grid_prob_ = Eigen::MatrixXd::Constant(height_, width_, 1.0 / (width_ * height_));
    }

    void updateProbability(double sensor_reading, const Eigen::Vector2d& agent_pos) {
        // Bayesian update step over spatial cells based on sensor reading
        for (int y = 0; y < height_; ++y) {
            for (int x = 0; x < width_; ++x) {
                Eigen::Vector2d cell_pos(x * resolution_, y * resolution_);
                double distance = (cell_pos - agent_pos).norm();
                double likelihood = computePlumeLikelihood(distance, sensor_reading);
                grid_prob_(y, x) *= likelihood;
            }
        }
        grid_prob_ /= grid_prob_.sum(); // Normalize probability distribution
    }

private:
    int width_, height_;
    double resolution_;
    Eigen::MatrixXd grid_prob_;

    double computePlumeLikelihood(double distance, double reading) {
        // Simplified plume dispersion Gaussian approximation model
        double expected_conc = 100.0 / (1.0 + distance * distance);
        return std::exp(-std::pow(reading - expected_conc, 2) / 20.0);
    }
};

Core Algorithm / Protocol Logic Implementation (Infotaxis & Surge-Cast)

import numpy as np

class InfotaxisAgent:
    def __init__(self, grid_shape, resolution):
        self.shape = grid_shape
        self.resolution = resolution
        self.belief = np.ones(grid_shape) / np.prod(grid_shape)
        
    def compute_expected_entropy_reduction(self, candidate_pos, hit_rate_model):
        """
        Evaluates expected Kullback-Leibler divergence / entropy drop
        for moving to a candidate position.
        """
        expected_entropy = 0.0
        # Integrate over possible sensor measurements (hit vs miss)
        for hit in [0.0, 1.0]:
            p_measurement = 0.0
            posterior_temp = np.zeros_like(self.belief)
            
            for y in range(self.shape[0]):
                for x in range(self.shape[1]):
                    source_pos = np.array([x, y]) * self.resolution
                    lambda_rate = hit_rate_model(source_pos, candidate_pos)
                    p_z_given_s = np.exp(-lambda_rate) if hit == 0.0 else 1.0 - np.exp(-lambda_rate)
                    
                    posterior_temp[y, x] = p_z_given_s * self.belief[y, x]
                    p_measurement += posterior_temp[y, x]
            
            if p_measurement > 1e-12:
                posterior_temp /= p_measurement
                entropy_after = -np.sum(posterior_temp * np.log(posterior_temp + 1e-15))
                expected_entropy += p_measurement * entropy_after
                
        current_entropy = -np.sum(self.belief * np.log(self.belief + 1e-15))
        return current_entropy - expected_entropy

Verification, Profiling & Unit Testing

import unittest
import numpy as np

class TestOdorLocalization(unittest.TestCase):
    def test_belief_normalization(self):
        grid_shape = (50, 50)
        agent = InfotaxisAgent(grid_shape, 1.0)
        self.assertAlmostEqual(np.sum(agent.belief), 1.0, places=5)

    def test_entropy_reduction_bounds(self):
        grid_shape = (10, 10)
        agent = InfotaxisAgent(grid_shape, 1.0)
        dummy_model = lambda s, c: 1.0 / (1.0 + np.linalg.norm(s - c))
        gain = agent.compute_expected_entropy_reduction(np.array([5.0, 5.0]), dummy_model)
        self.assertGreaterEqual(gain, 0.0)

if __name__ == '__main__':
    unittest.main()

Production Hardening & Common Failure Modes

class RobustSensorFilter {
public:
    explicit RobustSensorFilter(double alpha = 0.2) 
        : alpha_(alpha), filtered_value_(0.0), initialized_(false) {}

    double processReading(double raw_reading) {
        if (!initialized_) {
            filtered_value_ = raw_reading;
            initialized_ = true;
        } else {
            filtered_value_ = alpha_ * raw_reading + (1.0 - alpha_) * filtered_value_;
        }
        return filtered_value_;
    }

private:
    double alpha_;
    double filtered_value_;
    bool initialized_;
};