### Install Documentation Dependencies (Bash)

Source: https://github.com/decargroup/navlie/blob/main/README.rst

Installs the necessary Python packages for building the project's documentation. This is a prerequisite for generating the HTML documentation.

```bash
pip install -r docs/requirements.txt
```

--------------------------------

### Install navlie from source

Source: https://github.com/decargroup/navlie/blob/main/docs/source/index.rst

Commands to clone the navlie repository from GitHub and install the package in editable mode. This process automatically resolves dependencies, including the pymlg library.

```bash
git clone git@github.com:decargroup/navlie.git 
cd navlie && pip install -e .
```

--------------------------------

### Execute Batch Solver

Source: https://github.com/decargroup/navlie/blob/main/docs/source/tutorial/batch.ipynb

Runs the optimization solver on the configured problem and retrieves the optimized variables and summary statistics.

```python
opt_results = problem.solve()
variables_opt = opt_results["variables"]
print(opt_results["summary"])
```

--------------------------------

### Initialize SLAM States and Covariance in Python

Source: https://github.com/decargroup/navlie/blob/main/docs/source/tutorial/batch.ipynb

Initializes the robot's pose and a landmark's position as navlie states, and defines the measurement covariance matrix. This setup is used for evaluating the custom SLAM measurement model. It requires the navlie library and numpy.

```python
pose = nav.lib.SE2State(np.array([0.1, 1.0, 2.0]), state_id=pose_key_str)
landmark = nav.lib.VectorState(np.array([1.0, 2.0]), state_id=landmark_key_str)
R = np.identity(2) * 0.01

```

--------------------------------

### Generate Initial State Estimates

Source: https://github.com/decargroup/navlie/blob/main/docs/source/tutorial/batch.ipynb

Demonstrates how to generate initial robot trajectory estimates using dead-reckoning from odometry and initialize landmark positions by perturbing ground truth data.

```python
x0_hat = gt_poses[0].copy()
x0_hat.state_id = pose_key_str + "0"
init_pose_est = [x0_hat]
x = x0_hat.copy()
for k in range(len(input_list) -1):
    u = input_list[k]
    dt = input_list[k + 1].stamp - u.stamp 
    x = process_model.evaluate(x, u, dt)
    x.stamp = x.stamp + dt
    x.state_id = pose_key_str + str(k + 1)
    x.direction = "left"
    init_pose_est.append(x.copy())

init_landmark_est = []
for i, landmark in enumerate(landmarks):
    sigma_init = 0.4 if noise else 0.0
    perturbed_landmark = nav.lib.VectorState(landmark.value + np.random.randn(2) * sigma_init, state_id=landmark.state_id)
    init_landmark_est.append(perturbed_landmark)
```

--------------------------------

### Inspect Problem Variables in Python

Source: https://github.com/decargroup/navlie/blob/main/docs/source/tutorial/batch.ipynb

Demonstrates how to retrieve and print the initial keys from a problem's variable set to verify the structure of poses and landmarks.

```python
init_keys_list = list(problem.variables_init.keys())
print("First 10 keys: ")
print(init_keys_list[1:10])
print("Last 10 keys:")
print(init_keys_list[-10:])
```

--------------------------------

### Install navlie Package

Source: https://github.com/decargroup/navlie/blob/main/README.rst

Installs the navlie package and its dependencies, including pymlg for Lie group operations, using pip.

```bash
pip install -e .
```

--------------------------------

### Configure Nonlinear Least Squares Problem

Source: https://github.com/decargroup/navlie/blob/main/docs/source/tutorial/batch.ipynb

Initializes a batch optimization problem and registers variables including robot poses and landmark positions with unique identifiers.

```python
from navlie.batch.problem import Problem

problem = Problem()
for i, state in enumerate(init_pose_est):
    problem.add_variable(state.state_id, state)
for i, landmark in enumerate(init_landmark_est):
    problem.add_variable(landmark.state_id, landmark)
```

--------------------------------

### Import Navlie Library Components

Source: https://github.com/decargroup/navlie/blob/main/README.rst

Example of how to import core state types and models from the built-in Navlie library.

```python
from navlie.lib.states import VectorState, SE3State
from navlie.lib.models import RangePoseToAnchor, Altitude
```

--------------------------------

### Add Residuals to Optimization Problem

Source: https://github.com/decargroup/navlie/blob/main/docs/source/tutorial/batch.ipynb

Configures and adds prior, process, and measurement residuals to a Navlie problem instance. This setup is essential for defining the constraints and error terms before running the optimization solver.

```python
est_stamps = [state.stamp for state in init_pose_est]

init_cov = np.identity(3) * 1e-7
x0_hat = init_pose_est[0].copy()
prior_residual = PriorResidual(x0_hat.state_id, x0_hat.copy(), init_cov)
problem.add_residual(prior_residual)

for k in range(len(input_list) - 1):
    u = input_list[k]
    key_1 = f"{pose_key_str}{k}"
    key_2 = f"{pose_key_str}{k+1}"
    process_residual = ProcessResidual([key_1, key_2], process_model, u)
    problem.add_residual(process_residual)

from navlie.utils import find_nearest_stamp_idx
for k, meas in enumerate(meas_list):
    pose_idx = find_nearest_stamp_idx(est_stamps, meas.stamp)
    pose = init_pose_est[pose_idx]
    landmark_state_id = meas.model.landmark_id
    meas.model = PointRelativePositionSLAM(pose.state_id, landmark_state_id, R)
    meas_residual = PointRelativePositionResidual([pose.state_id, landmark_state_id], meas)
    problem.add_residual(meas_residual)
```

--------------------------------

### Create Measurement Model and Composite State

Source: https://github.com/decargroup/navlie/blob/main/docs/source/tutorial/batch.ipynb

Initializes a PointRelativePositionSLAM measurement model and a CompositeState for evaluating the model. The output 'y' represents the evaluated measurement.

```python
model = PointRelativePositionSLAM(pose_key_str, landmark_key_str, R)
state = nav.lib.CompositeState([pose, landmark])
y = model.evaluate(state)
print(y)
```

--------------------------------

### Generate Simulated Data with Navlie

Source: https://github.com/decargroup/navlie/blob/main/docs/source/tutorial/batch.ipynb

Sets up landmarks, measurement models, a process model, and an input profile to generate simulated trajectory data using Navlie's DataGenerator. It then plots the groundtruth poses and landmarks.

```python
# Now, create some landmarks arranged in a circle and create a list of
# measurement models, one for each landmark
landmark_positions = [np.array([3.0 * np.cos(theta), 3.0 * np.sin(theta)]) for theta in np.linspace(0, 2*np.pi, 10)]
landmarks = [nav.lib.VectorState(landmark, state_id=f"{landmark_key_str}{i}") for i, landmark in enumerate(landmark_positions)]
R = np.identity(2) * 0.1
meas_models = [PointRelativePosition(l.value, l.state_id, R) for l in landmarks]

# Create the process model
Q = np.identity(3) * 0.4
process_model = nav.lib.BodyFrameVelocity(Q)

# Input profile 
input_profile = lambda t, x: np.array(
    [np.cos(0.1 * t), 1.0, 0]
)

# Generate the data
x0 = nav.lib.SE2State(np.array([0, 0, 0]))
dg = nav.DataGenerator(
    process_model,
    input_profile,
    Q, 
    input_freq=100,
    meas_model_list=meas_models,
    meas_freq_list=[10] * len(meas_models),
)

gt_poses, input_list, meas_list = dg.generate(x0, start=0.0, stop=t_end, noise=noise)

# Plot the true state
import matplotlib.pyplot as plt
fig, ax = nav.plot_poses(gt_poses, step=100)
for landmark in landmarks:
    ax.plot(landmark.value[0], landmark.value[1], 'x')
ax.set_title("Groundtruth poses and landmarks")
ax.set_xlabel("x")
ax.set_ylabel("y")
plt.show()
```

--------------------------------

### Visualize Estimation Performance and Trajectories

Source: https://github.com/decargroup/navlie/blob/main/docs/source/tutorial/batch.ipynb

Generates diagnostic plots for the navigation system, including NEES, estimation errors, and comparative trajectory plots for groundtruth, initial, and optimized estimates. It also visualizes the sparsity pattern of the information matrix to analyze the optimization structure.

```python
gaussian_result_list = nav.GaussianResultList(
    [nav.GaussianResult(poses_results_list[i], gt_poses[i]) for i in range(len(poses_results_list))],
)

fig, axs = nav.plot_nees(gaussian_result_list)
fig, axs = nav.plot_error(gaussian_result_list)

opt_poses: typing.List[nav.lib.SE2State] = [state.state for state in poses_results_list]
fig, ax = nav.plot_poses(gt_poses, step = None, line_color='tab:blue', label="Groundtruth")
fig, ax = nav.plot_poses(init_pose_est, step=None, ax=ax, line_color='tab:red', label="Initial Estimate")
fig, ax = nav.plot_poses(opt_poses, step=None, ax=ax, line_color='tab:green', label="Optimized Estimate")

fig, ax = plt.subplots()
ax.spy(opt_results["info_matrix"])
plt.show()
```

--------------------------------

### Generate Random Vectors with Navlie

Source: https://context7.com/decargroup/navlie/llms.txt

Demonstrates the usage of Navlie's `randvec` function for generating random vectors based on a specified covariance matrix. It shows how to generate single vectors and multiple samples efficiently. The example includes verification of the empirical covariance against the target covariance.

```python
import numpy as np
import navlie as nav

# Generate random vector with given covariance
P = np.array([[1.0, 0.5], [0.5, 2.0]])
w = nav.randvec(P)  # Shape: (2, 1)

# Generate multiple samples efficiently
samples = nav.randvec(P, num_samples=1000)  # Shape: (2, 1000)

# Verify covariance
empirical_cov = np.cov(samples)
print(f"Target covariance:\n{P}")
print(f"Empirical covariance:\n{empirical_cov}")

```

--------------------------------

### Define Prior and Process Residuals in Python

Source: https://github.com/decargroup/navlie/blob/main/docs/source/tutorial/batch.ipynb

Implements custom residual classes for optimization problems. PriorResidual constrains the initial state, while ProcessResidual computes errors between consecutive states based on a process model.

```python
class PriorResidual(Residual):
    def __init__(self, keys, prior_state, prior_covariance):
        super().__init__(keys)
        self._cov = prior_covariance
        self._x0 = prior_state
        self._L = np.linalg.cholesky(np.linalg.inv(self._cov))

    def evaluate(self, states, compute_jacobians=None):
        x = states[0]
        error = self._L.T @ x.minus(self._x0)
        if compute_jacobians:
            jacobians = [self._L.T @ x.minus_jacobian(self._x0) if compute_jacobians[0] else None]
            return error, jacobians
        return error

class ProcessResidual(Residual):
    def __init__(self, keys, process_model, u):
        super().__init__(keys)
        self._process_model = process_model
        self._u = u

    def evaluate(self, states, compute_jacobians=None):
        x_km1, x_k = states[0], states[1]
        dt = x_k.stamp - x_km1.stamp
        x_k_hat = self._process_model.evaluate(x_km1.copy(), self._u, dt)
        L = self._process_model.sqrt_information(x_km1, self._u, dt)
        e = L.T @ x_k.minus(x_k_hat)
        if compute_jacobians:
            jac_list = [None, None]
            if compute_jacobians[0]: jac_list[0] = -L.T @ self._process_model.jacobian(x_km1, self._u, dt)
            if compute_jacobians[1]: jac_list[1] = L.T @ x_k.minus_jacobian(x_k_hat)
            return e, jac_list
        return e
```

--------------------------------

### Define Residual for Batch Estimation

Source: https://github.com/decargroup/navlie/blob/main/docs/source/tutorial/batch.ipynb

Imports the Residual class from navlie.batch.residuals, which is a fundamental component for defining error terms in nonlinear least squares problems for batch estimation.

```python
## Defining the residuals
from navlie.batch.residuals import Residual
```

--------------------------------

### Extract and Process State Estimates with Covariance

Source: https://github.com/decargroup/navlie/blob/main/docs/source/tutorial/batch.ipynb

Iterates through initial pose and landmark estimates to retrieve optimized states and their corresponding covariance blocks from the optimization problem. It returns lists of StateWithCovariance objects, defaulting to identity matrices if covariance computation is disabled.

```python
poses_results_list: typing.List[nav.types.StateWithCovariance] = []
for pose in init_pose_est:
    state = variables_opt[pose.state_id]
    if compute_covariance:
        cov = problem.get_covariance_block(pose.state_id, pose.state_id)
    else:
        cov = np.identity(3)
    poses_results_list.append(nav.types.StateWithCovariance(state, cov))

landmarks_results_list: typing.List[nav.types.StateWithCovariance] = []
for landmark in init_landmark_est:
    state = variables_opt[landmark.state_id]
    if compute_covariance:
        cov = problem.get_covariance_block(landmark.state_id, landmark.state_id)
    else:
        cov = np.identity(2)
    landmarks_results_list.append(nav.types.StateWithCovariance(state, cov))
```

--------------------------------

### Define SLAM Measurement Model in Python

Source: https://github.com/decargroup/navlie/blob/main/docs/source/tutorial/batch.ipynb

Defines a custom measurement model for SLAM problems where landmark positions are unknown. It calculates the relative position of a landmark in the robot's body frame and provides methods for evaluating the measurement, computing Jacobians, and returning the measurement covariance. Dependencies include navlie, numpy, and pymlg.

```python
import navlie as nav
import numpy as np
import typing 
from pymlg import SO3

### Parameters used for the example
# if true, the information matrix in the batch problem will be inverted to compute the covariance
compute_covariance = True 
# If false, will run without noise, and all states initialized to groundtruth 
noise = True 
# String keys to identify the states
pose_key_str = "x"
landmark_key_str = "l"
# The end time of the simulation
t_end = 20.0

### Defining the measurement model
class PointRelativePositionSLAM(nav.types.MeasurementModel):
    def __init__(self, pose_state_id: typing.Any, landmark_state_id: typing.Any, R: np.ndarray):
        self.pose_state_id = pose_state_id
        self.landmark_state_id = landmark_state_id
        self._R = R

    def evaluate(self, x: nav.CompositeState):
        pose: nav.lib.SE2State = x.get_state_by_id(self.pose_state_id)
        landmark: nav.lib.VectorState = x.get_state_by_id(self.landmark_state_id)

        r_a = pose.position.reshape((-1, 1))
        p_a = landmark.value.reshape((-1, 1))
        C_ab = pose.attitude
        return C_ab.T @ (p_a - r_a)

    def jacobians(self, x: nav.CompositeState):
        pose: nav.lib.SE2State = x.get_state_by_id(self._pose_state_id)
        landmark: nav.lib.VectorState = x.get_state_by_id(self._landmark_state_id)

        r_zw_a = pose.position.reshape((-1, 1))
        C_ab = pose.attitude
        r_pw_a = landmark.value.reshape((-1, 1))
        y = C_ab.T @ (r_pw_a - r_zw_a)

        # Compute Jacobian of measurement model with respect to the state
        if pose.direction == "right":
            pose_jacobian = pose.jacobian_from_blocks(
                attitude=-SO3.odot(y), position=-np.identity(r_zw_a.shape[0])
            )
        elif pose.direction == "left":
            pose_jacobian = pose.jacobian_from_blocks(
                attitude=-C_ab.T @ SO3.odot(r_pw_a), position=-C_ab.T
            )

        # Compute jacobian of measurement model with respect to the landmark
        landmark_jacobian = pose.attitude.T

        # Build full Jacobian
        state_ids = [state.state_id for state in x.value]
        jac_dict = {}
        jac_dict[state_ids[0]] = pose_jacobian
        jac_dict[state_ids[1]] = landmark_jacobian
        return x.jacobian_from_blocks(jac_dict)

    def covariance(self, x: nav.CompositeState):
        return self._R

```

--------------------------------

### Build HTML Documentation (Bash)

Source: https://github.com/decargroup/navlie/blob/main/README.rst

Generates the HTML version of the project's documentation. After running this command, the documentation can be viewed by opening the 'docs/index.html' file in a web browser.

```bash
make html
```

--------------------------------

### Run Monte Carlo Experiments with Navlie

Source: https://context7.com/decargroup/navlie/llms.txt

Demonstrates how to set up and run Monte Carlo experiments using Navlie's `monte_carlo` function. This involves defining a trial function that returns `GaussianResultList` and then aggregating results for statistical analysis. It utilizes `numpy` for numerical operations and `navlie` for state estimation components.

```python
import numpy as np
import navlie as nav
from navlie.lib.states import VectorState
from navlie.lib.models import SingleIntegrator, RangePointToAnchor

def run_trial(trial_number: int) -> nav.GaussianResultList:
    """Single trial function - must return GaussianResultList"""
    # Setup
    Q = 0.1 * np.eye(2)
    process_model = SingleIntegrator(Q)
    ekf = nav.ExtendedKalmanFilter(process_model)

    x0 = VectorState(np.array([0.0, 0.0]), stamp=0.0)
    P0 = np.eye(2)

    # Generate fresh data each trial
    dg = nav.DataGenerator(
        process_model,
        lambda t, x: np.array([np.sin(t), np.cos(t)]),
        Q, 100.0,
        [RangePointToAnchor([5.0, 5.0], 0.01)],
        [10.0],
    )
    gt_states, input_data, meas_data = dg.generate(x0, 0.0, 5.0, noise=True)

    # Run filter
    x0_noisy = x0.plus(nav.randvec(P0))
    estimates = nav.run_filter(ekf, x0_noisy, P0, input_data, meas_data,
                               disable_progress_bar=True)

    return nav.GaussianResultList.from_estimates(estimates, gt_states)

# Run Monte Carlo experiment
mc_results = nav.monte_carlo(
    trial=run_trial,
    num_trials=50,
    num_jobs=-1,  # Use all CPU cores
    verbose=10
)

# Access Monte Carlo statistics
print(f"Average NEES: {np.mean(mc_results.average_nees):.2f}")
print(f"Expected NEES: {mc_results.expected_nees[0]:.2f}")
print(f"Total RMSE: {np.mean(mc_results.total_rmse):.4f}")

# Get NEES bounds for Monte Carlo
mc_lower = mc_results.nees_lower_bound(0.99)
mc_upper = mc_results.nees_upper_bound(0.99)

```

--------------------------------

### Implement Custom Process Model

Source: https://github.com/decargroup/navlie/blob/main/README.rst

Demonstrates how to create a process model by inheriting from ProcessModel. Requires implementation of evaluate, covariance, and optionally jacobian methods.

```python
class SingleIntegrator(ProcessModel):
    def __init__(self, Q: np.ndarray):
        self._Q = Q

    def evaluate(self, x: VectorState, u: VectorInput, dt: float) -> np.ndarray:
        x.value = x.value + dt * u.value
        return x

    def jacobian(self, x: VectorState, u: VectorInput, dt: float) -> np.ndarray:
        return np.identity(x.dof)

    def covariance(self, x: VectorState, u: VectorInput, dt: float) -> np.ndarray:
        return dt**2 * self._Q
```

--------------------------------

### Implement Extended Kalman Filter (EKF) (Python)

Source: https://context7.com/decargroup/navlie/llms.txt

Demonstrates the implementation of the Extended Kalman Filter (EKF) using the navlie library. It covers setting up the process model, initializing the filter with an initial state and covariance, performing the predict step with inputs, and the correct step using a range measurement. Dependencies include numpy, navlie, navlie.lib.states, navlie.lib.models, and navlie.types.

```python
import numpy as np
import navlie as nav
from navlie.lib.states import VectorState, VectorInput
from navlie.lib.models import SingleIntegrator, RangePointToAnchor
from navlie.types import Measurement

# Setup
Q = 0.1 * np.eye(2)
process_model = SingleIntegrator(Q)
ekf = nav.ExtendedKalmanFilter(process_model, reject_outliers=False)

# Initial state with covariance
x0 = VectorState(np.array([0.0, 0.0]), stamp=0.0)
P0 = np.eye(2)
x = nav.StateWithCovariance(x0, P0)

# Predict step
u = VectorInput(np.array([1.0, 0.5]), stamp=0.0)
dt = 0.1
x = ekf.predict(x, u, dt)

# Correct step with range measurement
range_model = RangePointToAnchor([5.0, 5.0], R=0.01)
y = Measurement(value=7.0, stamp=0.1, model=range_model)
x = ekf.correct(x, y, u)

# Access results
print(f"Estimated position: {x.state.value}")
print(f"Covariance:\n{x.covariance}")
```

--------------------------------

### Define Custom Measurement Model for Data Generation

Source: https://github.com/decargroup/navlie/blob/main/docs/source/tutorial/batch.ipynb

Defines a custom PointRelativePosition measurement model inheriting from nav.types.MeasurementModel. This model calculates the relative position between a pose and a landmark and includes a covariance method.

```python
class PointRelativePosition(nav.types.MeasurementModel):
    def __init__(self, landmark_position: np.ndarray, landmark_id: int, R: np.ndarray):
        self.landmark_position = landmark_position.reshape((-1, 1))
        self.landmark_id = landmark_id
        self._R = R

    def evaluate(self, x: nav.lib.SE2State):
        r_a = x.position.reshape((-1, 1))
        p_a = self.landmark_position
        C_ab = x.attitude
        return C_ab.T @ (p_a - r_a)
    
    def covariance(self, x: nav.CompositeState):
        return self._R
```

--------------------------------

### Define Point Relative Position Residual

Source: https://github.com/decargroup/navlie/blob/main/docs/source/tutorial/batch.ipynb

Implements a custom residual class for point relative position measurements. It calculates the error between actual measurements and model predictions, optionally computing Jacobians for optimization.

```python
class PointRelativePositionResidual(Residual):
    def __init__(self, keys: typing.List[typing.Hashable], meas: nav.types.Measurement):
        super().__init__(keys)
        self.meas = meas
        self.sqrt_information = self.meas.model.sqrt_information([])

    def evaluate(self, states: typing.List[nav.types.State], compute_jacobians: typing.List[bool] = None) -> typing.Tuple[np.ndarray, typing.List[np.ndarray]]:
        eval_state = nav.CompositeState(states)
        y_check = self.meas.model.evaluate(eval_state)
        error = self.meas.value - y_check
        L = self.sqrt_information
        error = L.T @ error

        if compute_jacobians:
            jacobians = [None] * len(states)
            full_jac = -self.meas.model.jacobian(eval_state)
            if compute_jacobians[0]:
                jacobians[0] = L.T @ full_jac[:, :3]
            if compute_jacobians[1]:
                jacobians[1] = L.T @ full_jac[:, 3:]
            return error, jacobians
        return error
```

--------------------------------

### Generate synthetic navigation data with DataGenerator

Source: https://context7.com/decargroup/navlie/llms.txt

Demonstrates how to configure a DataGenerator with process and measurement models to simulate ground truth states, inputs, and measurements over a specified time range.

```python
range_models = [
    RangePointToAnchor([5.0, 5.0], R=0.01),
    RangePointToAnchor([-5.0, 5.0], R=0.01),
    RangePointToAnchor([0.0, -5.0], R=0.01),
]
range_freqs = [10.0, 10.0, 10.0]

dg = nav.DataGenerator(
    process_model=process_model,
    input_func=input_profile,
    input_covariance=Q,
    input_freq=100.0,
    meas_model_list=range_models,
    meas_freq_list=range_freqs,
)

x0 = VectorState(np.array([0.0, 0.0]), stamp=0.0)
gt_states, input_data, meas_data = dg.generate(
    x0, start=0.0, stop=10.0, noise=True
)
```

--------------------------------

### Initialize DataGenerator for Simulation

Source: https://github.com/decargroup/navlie/blob/main/docs/source/tutorial/composite.ipynb

Sets up a DataGenerator for simulating robot data. It takes the process model, an input function, input covariance, measurement models, and measurement frequencies as arguments. This is used to generate realistic data for testing and development.

```python
dg = nav.DataGenerator(
    process_model=process_model,                 
    input_func=lambda t, x: np.array([0.5, 0.3, 0.0, 0.0]), 
    input_covariance= Q,                         
    input_freq=50,                                
    meas_model_list=meas_models,                 
    meas_freq_list=[10, 10, 10, 10]
)
```

--------------------------------

### Create individual measurements with generate_measurement

Source: https://context7.com/decargroup/navlie/llms.txt

Shows how to generate single or batch measurements from a state and a defined measurement model. Useful for testing sensor models independently of a full filter pipeline.

```python
from navlie.datagen import generate_measurement
from navlie.lib.states import SE3State
from navlie.lib.models import PointRelativePosition

x = SE3State(np.eye(4), stamp=1.0)
model = PointRelativePosition(np.array([10.0, 5.0, 2.0]), 0.01 * np.eye(3))

# Single measurement
meas = generate_measurement(x, model, noise=True)

# Batch measurements
state_list = [SE3State(np.eye(4), stamp=float(i)) for i in range(5)]
meas_list = generate_measurement(state_list, model, noise=True)
```

--------------------------------

### Implement Custom Measurement Model

Source: https://github.com/decargroup/navlie/blob/main/README.rst

Demonstrates how to create a measurement model by inheriting from MeasurementModel. It calculates the expected measurement and its associated Jacobian.

```python
class RangePointToAnchor(MeasurementModel):
    def __init__(self, anchor_position: List[float], R: float):
        self._r_cw_a = np.array(anchor_position)
        self._R = np.array(R)

    def evaluate(self, x: VectorState) -> np.ndarray:
        r_zw_a = x.value
        y = np.linalg.norm(self._r_cw_a - r_zw_a)
        return y

    def jacobian(self, x: VectorState) -> np.ndarray:
        r_zw_a = x.value
        r_zc_a = r_zw_a - self._r_cw_a
        y = np.linalg.norm(r_zc_a)
        return r_zc_a.reshape((1, -1)) / y

    def covariance(self, x: VectorState) -> np.ndarray:
        return self._R
```

--------------------------------

### Implement Iterated Extended Kalman Filter (IEKF) (Python)

Source: https://context7.com/decargroup/navlie/llms.txt

Demonstrates the implementation of the Iterated Extended Kalman Filter (IEKF) using the navlie library, which re-linearizes the measurement model for improved accuracy. It covers setting up the SE3 process model, initializing the filter with convergence parameters, and performing predict and correct steps with a range measurement to an anchor. Dependencies include numpy, navlie, navlie.lib.states, navlie.lib.models, and navlie.types.

```python
import numpy as np
import navlie as nav
from navlie.lib.states import SE3State, VectorInput
from navlie.lib.models import BodyFrameVelocity, RangePoseToAnchor
from navlie.types import Measurement

# Setup SE3 state estimation
Q = 0.01 * np.eye(6)
process_model = BodyFrameVelocity(Q)

# Initialize IEKF with specific parameters

iekf = nav.IteratedKalmanFilter(
    process_model,
    step_tol=1e-4,      # Convergence tolerance
    max_iters=200,       # Maximum iterations
    line_search=True,    # Enable backtracking line search
    reject_outliers=False
)

# Initial pose estimate
x0 = SE3State(np.eye(4), stamp=0.0, direction="right")
P0 = 0.1 * np.eye(6)
x = nav.StateWithCovariance(x0, P0)

# Range measurement to anchor
anchor = [10.0, 0.0, 0.0]
tag_body_pos = [0.0, 0.0, 0.0]  # Tag at robot origin
range_model = RangePoseToAnchor(anchor, tag_body_pos, R=0.01)
y = Measurement(value=10.0, stamp=0.1, model=range_model)

# The predict and correct steps would follow here, similar to EKF but using the iekf object.
```

--------------------------------

### Calculate Process Model Jacobians in Python

Source: https://github.com/decargroup/navlie/blob/main/docs/source/tutorial/jacobians.ipynb

This snippet demonstrates the usage of the process_model object to compute Jacobians. It compares the analytical Jacobian method against the finite difference method for verification purposes.

```python
print("\nAnalyical:")
print(process_model.jacobian(x, u, dt))

print("\nFinite difference:")
print(process_model.jacobian_fd(x, u, dt))
```

--------------------------------

### Unscented Kalman Filter Implementation

Source: https://context7.com/decargroup/navlie/llms.txt

Configures and runs an Unscented Kalman Filter (UKF) using sigma points for nonlinear state estimation. It requires defining a process model, input covariance, and measurement models.

```python
import numpy as np
import navlie as nav
from navlie.lib.states import VectorState, VectorInput
from navlie.lib.models import SingleIntegrator, RangePointToAnchor
from navlie.types import Measurement

Q = 0.1 * np.eye(2)
process_model = SingleIntegrator(Q)
ukf = nav.UnscentedKalmanFilter(process_model, reject_outliers=False, iterate_mean=True)

x0 = VectorState(np.array([0.0, 0.0]), stamp=0.0)
P0 = np.eye(2)
x = nav.StateWithCovariance(x0, P0)

u = VectorInput(value=np.array([1.0, 0.5]), stamp=0.0, covariance=Q)

x = ukf.predict(x, u, dt=0.1)
range_model = RangePointToAnchor([5.0, 5.0], R=0.01)
y = Measurement(value=7.0, stamp=0.1, model=range_model)
x = ukf.correct(x, y, u)
```

--------------------------------

### Verify Process Model Jacobian using Finite Differences in Python

Source: https://github.com/decargroup/navlie/blob/main/docs/source/tutorial/jacobians.ipynb

This Python code snippet demonstrates how to verify the analytical Jacobian of a `WheeledRobot` process model against a numerically computed Jacobian using finite differences. It utilizes the `jacobian_fd()` method inherited from `navlie.ProcessModel` to perform this comparison, ensuring the correctness of the analytical implementation.

```python
x = VectorState([1,2,3]) 
u = VectorInput([0.1, 0.2]) 
dt = 0.1 

print("Analyical:")
print(process_model.jacobian(x, u, dt))

print("\nFinite difference:")
print(process_model.jacobian_fd(x, u, dt))
```

--------------------------------

### Implement Gravitometer Measurement Model for Attitude (Python)

Source: https://context7.com/decargroup/navlie/llms.txt

Measures the gravity vector in the body frame for attitude estimation using the Gravitometer model. It demonstrates setting up the measurement covariance and world gravity vector, evaluating the measurement from an SO3 state, and computing the Jacobian with respect to the attitude. Dependencies include numpy, navlie.lib.models.Gravitometer, and navlie.lib.states.SO3State.

```python
import numpy as np
from navlie.lib.models import Gravitometer
from navlie.lib.states import SO3State

# Define measurement covariance and gravity vector
R = 0.01 * np.eye(3)
g_world = [0, 0, -9.80665]  # Gravity in world frame
model = Gravitometer(R, gravity_vector=g_world)

# SO3 attitude state
C = np.eye(3)  # Identity rotation
x = SO3State(C, stamp=0.0, direction="right")

# Measurement: y = C_ab.T @ g_world
y = model.evaluate(x)  # Gravity resolved in body frame
G = model.jacobian(x)  # Jacobian w.r.t. attitude
```

--------------------------------

### Data Generation for Simulation

Source: https://context7.com/decargroup/navlie/llms.txt

Provides a framework for defining process models and input profiles to generate synthetic trajectories and measurements for estimator testing.

```python
import numpy as np
from navlie.lib.models import SingleIntegrator

Q = 0.1 * np.eye(2)
process_model = SingleIntegrator(Q)

def input_profile(t, x):
    return np.array([np.sin(t), np.cos(t)])
```

--------------------------------

### Run navlie Integration Tests

Source: https://github.com/decargroup/navlie/blob/main/README.rst

Executes integration tests for the navlie library using pytest. Specific test files can be targeted using file paths.

```bash
pytest tests
```

```bash
pytest -ra tests/integration/filename.py
```

--------------------------------

### Implement Wheeled Robot Process Model

Source: https://github.com/decargroup/navlie/blob/main/docs/source/tutorial/traditional.ipynb

Implements a custom process model for a wheeled robot by inheriting from navlie.ProcessModel. It defines the `evaluate` method for state transition and `input_covariance` for input noise.

```python
from navlie.lib import VectorInput

Q = np.eye(2) * 0.1**2 # Input noise covariance with 0.1 m/s of standard deviation

class WheeledRobot(nav.ProcessModel):
    def __init__(self, input_covariance):
        self.Q = input_covariance

    def evaluate(self, x: VectorState, u: VectorInput, dt: float) -> VectorState:
        x_next = x.copy()
        x_next.value[0] += u.value[0] * dt
        x_next.value[1] += u.value[1] * dt * np.cos(x.value[0])
        x_next.value[2] += u.value[1] * dt * np.sin(x.value[0])
        return x_next

    def input_covariance(self, x: VectorState, u: VectorInput, dt: float) -> np.ndarray:
        return self.Q
    
process_model = WheeledRobot(Q) # instantiate it
```

--------------------------------

### Implement RangeToLandmark Measurement Model

Source: https://github.com/decargroup/navlie/blob/main/docs/source/tutorial/traditional.ipynb

Implements a measurement model for range to a landmark by inheriting from navlie.MeasurementModel. It defines `evaluate` to calculate the range and `covariance` for measurement noise.

```python
class RangeToLandmark(nav.MeasurementModel):
    def __init__(self, landmark_position: np.ndarray):
        self.landmark_position = landmark_position

    def evaluate(self, x: VectorState) -> np.ndarray:
        return np.linalg.norm(x.value[1:] - self.landmark_position)
    
    def covariance(self, x: VectorState) -> np.ndarray:
        return 0.1**2
    
landmarks = np.array([[1, 1], [1, 2], [2, 2], [2, 1]])
meas_models = [RangeToLandmark(landmark) for landmark in landmarks]
```

--------------------------------

### Compute Jacobians Numerically with Navlie

Source: https://context7.com/decargroup/navlie/llms.txt

Illustrates how to compute Jacobians numerically using Navlie's `jacobian` function. This is useful for verification or when analytical Jacobians are not readily available. It supports various differentiation methods like 'forward', 'central', and 'complex-step'. The function can operate on custom functions and Navlie states like `SE3State` or standard NumPy arrays.

```python
import numpy as np
import navlie as nav
from navlie.lib.states import SE3State

# Define a function of a state
def measurement_func(T: SE3State):
    """Returns normalized body-frame velocity direction"""
    C = T.attitude
    v = T.velocity if hasattr(T, 'velocity') else T.position
    v_body = C.T @ v
    return v_body / np.linalg.norm(v_body)

# Compute Jacobian
T = SE3State(np.eye(4), direction="right")
T.value[0:3, 3] = [1.0, 2.0, 3.0]  # Set position

jac = nav.jacobian(
    fun=measurement_func,
    x=T,
    step_size=1e-6,
    method="forward"  # or "central", "cs" (complex-step)
)
print(f"Jacobian shape: {jac.shape}")

# Works with numpy arrays too
def simple_func(x):
    A = np.array([[1, 2], [3, 4]])
    return A @ x

x = np.array([[1.0], [2.0]])
jac = nav.jacobian(simple_func, x)
print(f"Jacobian:\n{jac}")  # Should be [[1, 2], [3, 4]]

```

--------------------------------

### Define SE3 Process Model and Propagate State (Python)

Source: https://context7.com/decargroup/navlie/llms.txt

Defines a process noise covariance for SE3 states and uses a BodyFrameVelocity model to propagate the state. It also calculates the analytical Jacobians for the propagation. Dependencies include numpy and navlie.lib.models.BodyFrameVelocity.

```python
import numpy as np
from navlie.lib.models import BodyFrameVelocity
from navlie.lib.states import SE3State, VectorInput

# Define process noise covariance (6x6 for SE3)
Q = 0.01 * np.eye(6)
process_model = BodyFrameVelocity(Q)

# Create pose state and body-frame velocity input
x = SE3State(np.eye(4), stamp=0.0, direction="right")
# Input: [angular_velocity (3), linear_velocity (3)]
u = VectorInput(np.array([0.0, 0.0, 0.1, 1.0, 0.0, 0.0]), stamp=0.0)

# Propagate: T_new = T @ Exp(u * dt)
dt = 0.1
x_new = process_model.evaluate(x, u, dt)

# The model provides analytical Jacobians for efficiency
F = process_model.jacobian(x, u, dt)
Q_k = process_model.covariance(x, u, dt)
```

--------------------------------

### Execute Extended Kalman Filter

Source: https://github.com/decargroup/navlie/blob/main/docs/source/tutorial/traditional.ipynb

Demonstrates the stateless filtering loop using the ExtendedKalmanFilter. It iterates through input data to perform prediction steps and incorporates measurements via correction steps.

```python
kalman_filter = nav.ExtendedKalmanFilter(process_model)
P0 = np.diag([0.1**2, 1**2, 1**2])
x = nav.StateWithCovariance(x, P0)

meas_idx = 0
y = meas_data[meas_idx]
estimates = []
for k in range(len(input_data) - 1):
	u = input_data[k]
	while y.stamp < input_data[k + 1].stamp and meas_idx < len(meas_data):
		x = kalman_filter.correct(x, y, u)
		meas_idx += 1
		if meas_idx < len(meas_data):
			y = meas_data[meas_idx]
	dt = input_data[k + 1].stamp - x.state.stamp
	x = kalman_filter.predict(x, u, dt)
	estimates.append(x.copy())
```

--------------------------------

### SE3State: Represent and Manipulate 3D Rigid Body Poses

Source: https://context7.com/decargroup/navlie/llms.txt

The SE3State class models 3D poses using the SE(3) Lie group, storing a 4x4 transformation matrix. It supports initialization from exponential coordinates or matrices, provides access to attitude and position, and enables Lie group operations for perturbations and error calculations.

```python
import numpy as np
from navlie.lib.states import SE3State
from pymlg import SE3

# Create from exponential coordinates [rotation (3), translation (3)]
xi = np.array([0.1, 0.2, 0.3, 1.0, 2.0, 3.0])  # [omega, position]
x = SE3State(xi, stamp=0.0, state_id="pose", direction="right")

# Or create from a 4x4 transformation matrix
T = SE3.Exp(xi)
x = SE3State(T, stamp=0.0)

# Access pose components
C_ab = x.attitude    # 3x3 rotation matrix (DCM)
r = x.position       # 3D position vector

# Lie group operations with right perturbation
dx = np.array([0.01, 0.02, 0.03, 0.1, 0.2, 0.3])  # Small perturbation
x_perturbed = x.plus(dx)  # T_new = T @ SE3.Exp(dx)

# Compute error between states
error = x_perturbed.minus(x)  # Returns 6D error vector
```

--------------------------------

### Batch MAP Estimation

Source: https://context7.com/decargroup/navlie/llms.txt

Solves a full batch estimation problem using Gauss-Newton or Levenberg-Marquardt optimization. It processes lists of inputs and measurements to estimate states over a trajectory.

```python
estimator = nav.BatchEstimator(solver_type="LM", max_iters=100, verbose=True)
Q = 0.1 * np.eye(2)
process_model = SingleIntegrator(Q)

input_data = [VectorInput(np.array([1.0, 0.5]), stamp=t * 0.1) for t in range(100)]
meas_data = [Measurement(value=7.0, stamp=t * 0.1, model=RangePointToAnchor([5.0, 5.0], 0.01)) for t in range(100)]

estimates, opt_results = estimator.solve(
    VectorState(np.array([0.0, 0.0]), stamp=0.0), np.eye(2), 
    input_data, meas_data, process_model, return_opt_results=True
)
```

--------------------------------

### Extended Kalman Filter for Pose and Bias Estimation

Source: https://github.com/decargroup/navlie/blob/main/docs/source/tutorial/composite.ipynb

This snippet demonstrates the initialization and execution of an Extended Kalman Filter for estimating the state of a system, likely a robot, including its pose and biases. It involves defining the process model, initial state covariance, and then iterating through measurements and inputs to predict and correct the state estimate. Finally, it generates results and plots estimation errors and NEES.

```python
state_data, input_data, meas_data = dg.generate(x, start=0, stop=30, noise=True)

# First, define the filter
kalman_filter = nav.ExtendedKalmanFilter(process_model)
P0 = np.diag([0.1**2, 1**2, 1**2, 0.1**2, 0.1**2])  # Initial covariance
x = nav.StateWithCovariance(x, P0)  # Estimate and covariance in one container

meas_idx = 0
y = meas_data[meas_idx]
estimates = []
for k in range(len(input_data) - 1):
    u = input_data[k]

    # Fuse any measurements that have occurred.
    while y.stamp < input_data[k + 1].stamp and meas_idx < len(meas_data):
        x = kalman_filter.correct(x, y, u)

        # Load the next measurement
        meas_idx += 1
        if meas_idx < len(meas_data):
            y = meas_data[meas_idx]

    # Predict until the next input is available
    dt = input_data[k + 1].stamp - x.state.stamp
    x = kalman_filter.predict(x, u, dt)

    estimates.append(x.copy())


results = nav.GaussianResultList.from_estimates(estimates, state_data)

import matplotlib.pyplot as plt
fig, axs = nav.plot_error(results)
axs[0,0].set_title("Pose Estimation Errors")
axs[0,0].set_ylabel("theta (rad)")
axs[1,0].set_ylabel("x (m)")
axs[2,0].set_ylabel("y (m)")
axs[2,0].set_xlabel("Time (s)")
axs[0, 1].set_title("Bias Estimation Errors")
axs[0, 1].set_ylabel("ang. vel. (rad/s)")
axs[1, 1].set_ylabel("forward vel. (m/s)")
axs[1, 1].set_xlabel("Time (s)")

fig, ax = nav.plot_nees(results)
ax.set_title("NEES")
ax.set_xlabel("Time (s)")
plt.show()
```

--------------------------------

### Execute filter pipelines with run_filter

Source: https://context7.com/decargroup/navlie/llms.txt

Provides a streamlined approach to running a predict-correct filter loop using pre-generated input and measurement data.

```python
ekf = nav.ExtendedKalmanFilter(process_model)
x0 = VectorState(np.array([0.0, 0.0]), stamp=0.0)
P0 = np.eye(2)

results = nav.run_filter(
    ekf, x0, P0, input_data, meas_data, disable_progress_bar=False
)

print(f"Final estimate: {results[-1].state.value}")
```

--------------------------------

### SingleIntegrator: Propagate Position States with Velocity Inputs

Source: https://context7.com/decargroup/navlie/llms.txt

The SingleIntegrator process model propagates position states using discrete-time dynamics based on velocity inputs. It allows defining process noise covariance, creating states and inputs, and provides methods to evaluate state propagation, compute Jacobians, and derive covariance matrices.

```python
import numpy as np
from navlie.lib.models import SingleIntegrator
from navlie.lib.states import VectorState, VectorInput

# Define process noise covariance
Q = 0.1 * np.eye(2)  # 2D position state
process_model = SingleIntegrator(Q)

# Create state and input
x = VectorState(np.array([0.0, 0.0]), stamp=0.0)
u = VectorInput(np.array([1.0, 0.5]), stamp=0.0)  # Velocity [vx, vy]

# Propagate state forward
dt = 0.1  # Time step in seconds
x_new = process_model.evaluate(x, u, dt)
# x_new.value = [0.1, 0.05]  (x + dt * u)

# Get Jacobian and covariance for EKF
F = process_model.jacobian(x, u, dt)   # Identity matrix for single integrator
Q_k = process_model.covariance(x, u, dt)  # dt^2 * Q
```