Trajectory

trajectory

Trajectory

Trajectory(
    positions: Positions,
    rotations: Union[Rotations, None] = None,
    timestamps: Union[ndarray, None] = None,
    name: str = "",
    path_lengths: Union[ndarray, None] = None,
    velocity_xyz: Union[ndarray, None] = None,
    sorting: Sorting = TIME,
)

Class representing a trajectory containing synchronized position, orientation, and time data.

Attributes:

• positions (Positions) –

  Container for spatial coordinates and coordinate reference system (EPSG) data.

• rotations (Rotations) –

  Container for orientation data (quaternions), or None if not provided.

• timestamps (ndarray) –

  A 1D array of timestamps corresponding to each pose.

• name (str) –

  An identifier string for the trajectory.

• path_lengths (ndarray) –

  A 1D array of cumulative path lengths starting from zero.

• sorting (Sorting) –

  The current sorting strategy (Sorting.TIME or Sorting.ARC_LENGTH).

Initialize a Trajectory object.

Parameters:

• positions (Positions) –

  Container for spatial coordinates and coordinate reference system (EPSG) data.

• rotations (Rotations | None, default: None ) –

  Container for orientation data (quaternions). Defaults to None.

• timestamps (ndarray | None, default: None ) –

  Array of timestamps. If None, a range index is used.

• name (str, default: '' ) –

  Name of the trajectory. Defaults to generic counter name.

• path_lengths (Union[ndarray, None], default: None ) –

  Pre-calculated path lengths. If None, they are computed from xyz.

• velocity_xyz (Union[ndarray, None], default: None ) –

  Pre-calculated 3D velocities. If None, they are computed via gradient.

• sorting (Sorting, default: TIME ) –

  Definition of the sorting logic (TIME or ARC_LENGTH). Defaults to Sorting.TIME.

Raises:

• TrajectoryError –

  If the number of positions and rotations do not match.

Source code in trajectopy\core\trajectory.py

def __init__(
    self,
    positions: Positions,
    rotations: Union[Rotations, None] = None,
    timestamps: Union[np.ndarray, None] = None,
    name: str = "",
    path_lengths: Union[np.ndarray, None] = None,
    velocity_xyz: Union[np.ndarray, None] = None,
    sorting: Sorting = Sorting.TIME,
) -> None:
    """
    Initialize a Trajectory object.

    Args:
        positions (Positions): Container for spatial coordinates and coordinate reference system (EPSG) data.
        rotations (Rotations | None, optional): Container for orientation data (quaternions). Defaults to None.
        timestamps (np.ndarray | None, optional): Array of timestamps. If None, a range index is used.
        name (str, optional): Name of the trajectory. Defaults to generic counter name.
        path_lengths (Union[np.ndarray, None], optional): Pre-calculated path lengths. If None, they are computed from xyz.
        velocity_xyz (Union[np.ndarray, None], optional): Pre-calculated 3D velocities. If None, they are computed via gradient.
        sorting (Sorting, optional): Definition of the sorting logic (TIME or ARC_LENGTH). Defaults to Sorting.TIME.

    Raises:
        TrajectoryError: If the number of positions and rotations do not match.
    """
    # check dimensions
    if rotations is not None and len(positions) != len(rotations):
        raise TrajectoryError(
            f"Number of positions ({len(positions)}) and rotations ({len(rotations)}) do not match!"
        )

    self.sorting = sorting

    # pose
    self.positions = positions
    self.rotations = rotations

    if not isinstance(timestamps, np.ndarray):
        timestamps = np.array(timestamps) if timestamps is not None else None

    self.timestamps = np.arange(0, len(positions)) if timestamps is None else timestamps

    if velocity_xyz is not None and len(velocity_xyz) == len(self.positions):
        self._velocity_xyz = velocity_xyz
    else:
        self._velocity_xyz = gradient_3d(xyz=self.positions.xyz, tstamps=self.timestamps)
        logger.info("Speeds were not provided or had wrong dimensions. Speeds were computed instead.")

    if path_lengths is not None and len(path_lengths) == len(self.positions):
        self.path_lengths = path_lengths
    else:
        self.path_lengths = self.init_path_lengths()
        logger.info("Path lengths were not provided or had wrong dimensions. Path lengths were computed instead.")

    self.name = name or f"Trajectory {Trajectory._counter}"

    Trajectory._counter += 1

has_orientation property

has_orientation: bool

Returns True if valid rotation data is available.

is_unix_time property

is_unix_time: bool

Checks if the supplied trajectory has (likely) unix timestamps as seconds.

total_length property

total_length: float

Returns the total cumulative path length of the trajectory in meters.

sort_switching_index property

sort_switching_index: ndarray

Returns an array of indices that would switch the current sorting (e.g., unsort the data).

sorting_index property

sorting_index: ndarray

Returns the indices used to sort the trajectory based on the current sorting attribute (Time or Path Length).

index property

index: ndarray

Returns the independent variable currently parameterizing the trajectory. This is either the Timestamp vector or the Path Length vector, depending on self.sorting.

datetimes property

datetimes: ndarray

Returns the timestamps converted to Pandas datetime objects (unit='s').

index_unit property

index_unit: str

Returns the unit string of the current index ('s' for Time, 'm' for Path Length).

index_label property

index_label: str

Returns the label string of the current index (e.g., 'time [s]').

data_rate property

data_rate: float

Calculates the average data rate (frequency in Hz) based on timestamp differences.

velocity_xyz property writable

velocity_xyz: ndarray

Returns the 3D velocity vectors. If not set manually, they are computed via gradient of the positions over time.

absolute_velocity property

absolute_velocity: ndarray

Returns the norm (magnitude) of the 3D velocity vectors.

xyz property

xyz: ndarray

Returns the XYZ coordinates sorted according to the current sorting strategy. Note: This differs from self.positions.xyz, which retains the original order.

quat property

quat: ndarray

Returns the quaternions sorted according to the current sorting strategy. Returns zeros if no rotations are present.

rpy property

rpy: ndarray

Returns the Roll-Pitch-Yaw angles sorted according to the current sorting strategy.

se3 property writable

se3: List[ndarray]

Returns a list of SE3 poses (4x4 homogeneous transformation matrices).

from_file classmethod

from_file(
    filename: str, io_stream: bool = False
) -> Trajectory

Create a trajectory instance from a file.

The file is expected to be a CSV-like format. It handles extraction of timestamps, xyz positions, rotations, path lengths, and velocities via ascii trajectory reader.

Parameters:

• filename (str) –

  Path to the file or string content if io_stream is True.

• io_stream (bool, default: False ) –

  If True, filename is treated as the raw string content of the file/stream. Defaults to False.

Returns:

• Trajectory ( Trajectory ) –

  The loaded trajectory object.

Source code in trajectopy\core\trajectory.py

@classmethod
def from_file(cls, filename: str, io_stream: bool = False) -> "Trajectory":
    """
    Create a trajectory instance from a file.

    The file is expected to be a CSV-like format. It handles extraction of
    timestamps, xyz positions, rotations, path lengths, and velocities via `ascii` trajectory reader.

    Args:
        filename (str): Path to the file or string content if io_stream is True.
        io_stream (bool, optional): If True, `filename` is treated as the raw string content
                                    of the file/stream. Defaults to False.

    Returns:
        Trajectory: The loaded trajectory object.
    """
    if io_stream:
        header_data, trajectory_data = ascii.read_string(filename, dtype=object)
    else:
        header_data, trajectory_data = ascii.read_data(filename, dtype=object)

    tstamps = ascii.extract_trajectory_timestamps(header_data=header_data, trajectory_data=trajectory_data)
    positions = ascii.extract_trajectory_positions(header_data=header_data, trajectory_data=trajectory_data)
    path_lengths = ascii.extract_trajectory_path_lengths(header_data=header_data, trajectory_data=trajectory_data)
    velocity_xyz = ascii.extract_trajectory_velocity_xyz(header_data=header_data, trajectory_data=trajectory_data)
    rotations = ascii.extract_trajectory_rotations(header_data=header_data, trajectory_data=trajectory_data)

    return Trajectory(
        timestamps=tstamps,
        positions=positions,
        rotations=rotations,
        name=header_data.name,
        path_lengths=path_lengths,
        velocity_xyz=velocity_xyz,
        sorting=Sorting.from_str(header_data.sorting),
    )

from_arrays classmethod

from_arrays(
    xyz: ndarray,
    quat: ndarray | None = None,
    rpy: ndarray | None = None,
    epsg: int = 0,
    **kwargs
) -> Trajectory

Factory: Handles creation from raw numpy arrays.

Source code in trajectopy\core\trajectory.py

@classmethod
def from_arrays(
    cls, xyz: np.ndarray, quat: np.ndarray | None = None, rpy: np.ndarray | None = None, epsg: int = 0, **kwargs
) -> "Trajectory":
    """Factory: Handles creation from raw numpy arrays."""

    pos_obj = Positions(xyz=xyz, epsg=epsg)

    rot_obj = None
    if quat is not None and rpy is not None:
        raise TrajectoryError("Provide quat OR rpy, not both.")

    if quat is not None:
        rot_obj = Rotations.from_quat(quat)
    elif rpy is not None:
        rot_obj = Rotations.from_euler(seq="xyz", angles=rpy)

    return cls(positions=pos_obj, rotations=rot_obj, **kwargs)

__str__

__str__() -> str

Returns a formatted string summary of the trajectory, including name, length, EPSG, and data rate.

Source code in trajectopy\core\trajectory.py

def __str__(self) -> str:
    """
    Returns a formatted string summary of the trajectory, including name, length, EPSG, and data rate.
    """
    width = 24
    return (
        f"\n _______________________________________________________\n"
        f"| ------------------ Trajectory Info ------------------ |\n"
        f"| Name:                         {self.name:<{width}}|\n"
        f"| Number of poses:              {len(self):<{width}}|\n"
        f"| Orientation available:        {'yes' if self.has_orientation else 'no':<{width}}|\n"
        f"| EPSG:                         {self.positions.epsg:<{width}}|\n"
        f"| Length [m]:                   {self.total_length:<{width}.3f}|\n"
        f"| Data rate [Hz]:               {self.data_rate:<{width}.3f}|\n"
        f"| Function of:                  {self.index_label:<{width}}|\n"
        f"|_______________________________________________________|\n"
    )

__len__

__len__() -> int

Returns the number of poses in the trajectory.

Source code in trajectopy\core\trajectory.py

def __len__(self) -> int:
    """Returns the number of poses in the trajectory."""
    return len(self.positions.xyz)

__eq__

__eq__(other: Trajectory) -> bool

Checks equality between two trajectories using np.allclose for numerical arrays. Compares positions, rotations, timestamps, path lengths, velocities, and names.

Source code in trajectopy\core\trajectory.py

def __eq__(self, other: "Trajectory") -> bool:
    """
    Checks equality between two trajectories using `np.allclose` for numerical arrays.
    Compares positions, rotations, timestamps, path lengths, velocities, and names.
    """
    if self.rotations is not None and other.rotations is not None:
        rot_equal = np.allclose(self.rotations.as_quat(), other.rotations.as_quat())
    elif self.rotations is None and other.rotations is None:
        rot_equal = True
    else:
        rot_equal = False

    return (
        np.allclose(self.positions.xyz, other.positions.xyz)
        and rot_equal
        and np.allclose(self.timestamps, other.timestamps)
        and np.allclose(self.path_lengths, other.path_lengths)
        and np.allclose(self._velocity_xyz, other._velocity_xyz)
        and self.name == other.name
    )

copy

copy() -> Trajectory

Returns a deep copy of the trajectory instance.

Source code in trajectopy\core\trajectory.py

def copy(self) -> "Trajectory":
    """Returns a deep copy of the trajectory instance."""
    return copy.deepcopy(self)

init_path_lengths

init_path_lengths()

Computes cumulative path lengths based on Euclidean distances between consecutive local coordinates.

Source code in trajectopy\core\trajectory.py

def init_path_lengths(self):
    """Computes cumulative path lengths based on Euclidean distances between consecutive local coordinates."""
    return lengths_from_xyz(self.positions.to_local(inplace=False).xyz)

overlaps_with

overlaps_with(other: Trajectory) -> bool

Checks if the time span of this trajectory overlaps with another.

Parameters:

• other (Trajectory) –

  The trajectory to compare against.

Returns:

• bool ( bool ) –

  True if the time ranges overlap, False otherwise.

Source code in trajectopy\core\trajectory.py

def overlaps_with(self, other: "Trajectory") -> bool:
    """
    Checks if the time span of this trajectory overlaps with another.

    Args:
        other (Trajectory): The trajectory to compare against.

    Returns:
        bool: True if the time ranges overlap, False otherwise.
    """
    start_test = self.timestamps[0]
    end_test = self.timestamps[-1]
    start_ref = other.timestamps[0]
    end_ref = other.timestamps[-1]

    return (start_test <= end_ref and end_test >= start_ref) or (start_ref <= end_test and end_ref >= start_test)

crop

crop(
    t_start: float,
    t_end: float,
    inverse: bool = False,
    inplace: bool = True,
) -> Trajectory

Crops (or cuts) the trajectory based on a time window.

Parameters:

• t_start (float) –

  Start timestamp of the window.

• t_end (float) –

  End timestamp of the window.

• inverse (bool, default: False ) –

  If True, removes data inside the window (cutting). If False, keeps data inside the window (cropping). Defaults to False.

• inplace (bool, default: True ) –

  If True, modifies self. If False, returns a new instance. Defaults to True.

Returns:

• Trajectory ( Trajectory ) –

  The modified or new trajectory instance.

Source code in trajectopy\core\trajectory.py

def crop(self, t_start: float, t_end: float, inverse: bool = False, inplace: bool = True) -> "Trajectory":
    """
    Crops (or cuts) the trajectory based on a time window.

    Args:
        t_start (float): Start timestamp of the window.
        t_end (float): End timestamp of the window.
        inverse (bool, optional): If True, removes data *inside* the window (cutting).
                                  If False, keeps data *inside* the window (cropping). Defaults to False.
        inplace (bool, optional): If True, modifies self. If False, returns a new instance. Defaults to True.

    Returns:
        Trajectory: The modified or new trajectory instance.
    """
    # filter to t_start and t_end
    if inverse:
        filt = [not t_start <= tstamps <= t_end for tstamps in self.timestamps]
    else:
        filt = [t_start <= tstamps <= t_end for tstamps in self.timestamps]

    return self.mask(mask=filt, inplace=inplace)

intersect

intersect(
    timestamps: ndarray,
    max_gap_size: float = 10.0,
    inplace: bool = True,
) -> Trajectory

Filters the trajectory to overlap with a reference timestamp vector.

This method finds the common time span between self and the reference timestamps, crops self to that span, and then filters points that are either exact matches or exist within valid gaps defined by max_gap_size.

Parameters:

• timestamps (ndarray) –

  The reference timestamps to intersect with.

• max_gap_size (float, default: 10.0 ) –

  The maximum allowed time gap (in seconds) between reference timestamps to include trajectory points. Defaults to 10.0.

• inplace (bool, default: True ) –

  If True, modifies self. Defaults to True.

Raises:

• ValueError –

  If the time spans do not overlap.

Returns:

• Trajectory ( Trajectory ) –

  The intersected trajectory.

Source code in trajectopy\core\trajectory.py

def intersect(self, timestamps: np.ndarray, max_gap_size: float = 10.0, inplace: bool = True) -> "Trajectory":
    """
    Filters the trajectory to overlap with a reference timestamp vector.

    This method finds the common time span between self and the reference `timestamps`,
    crops self to that span, and then filters points that are either exact matches
    or exist within valid gaps defined by `max_gap_size`.

    Args:
        timestamps (np.ndarray): The reference timestamps to intersect with.
        max_gap_size (float, optional): The maximum allowed time gap (in seconds) between
                                        reference timestamps to include trajectory points. Defaults to 10.0.
        inplace (bool, optional): If True, modifies self. Defaults to True.

    Raises:
        ValueError: If the time spans do not overlap.

    Returns:
        Trajectory: The intersected trajectory.
    """
    traj_self = self if inplace else self.copy()
    time_span = common_time_span(tstamps1=timestamps, tstamps2=traj_self.timestamps)

    if time_span is None:
        raise ValueError("intersect_both: Timespans do not overlap!")

    traj_self.crop(t_start=time_span[0], t_end=time_span[1])

    tstamps_sorted = np.sort(timestamps)
    lower_neighbor_list = np.searchsorted(tstamps_sorted, traj_self.timestamps, side="right") - 1

    filter_index = [
        idx
        for idx, (tstamp, lower_neighbor) in enumerate(zip(traj_self.timestamps, lower_neighbor_list))
        if (
            tstamps_sorted[lower_neighbor] == tstamp
            or (tstamps_sorted[lower_neighbor + 1] - tstamps_sorted[lower_neighbor]) <= max_gap_size
        )
    ]
    traj_self.mask(np.array(filter_index, dtype=int))

    return traj_self

mask

mask(
    mask: Union[list, ndarray], inplace: bool = True
) -> Trajectory

Applies a boolean mask or index array to filter all trajectory components.

Filtered components include: timestamps, positions, rotations, path lengths, and velocities.

Parameters:

• mask (Union[list, ndarray]) –

  Boolean array or list of indices to keep.

• inplace (bool, default: True ) –

  If True, modifies self. Defaults to True.

Returns:

• Trajectory ( Trajectory ) –

  The masked trajectory.

Source code in trajectopy\core\trajectory.py

def mask(self, mask: Union[list, np.ndarray], inplace: bool = True) -> "Trajectory":
    """
    Applies a boolean mask or index array to filter all trajectory components.

    Filtered components include: timestamps, positions, rotations, path lengths, and velocities.

    Args:
        mask (Union[list, np.ndarray]): Boolean array or list of indices to keep.
        inplace (bool, optional): If True, modifies self. Defaults to True.

    Returns:
        Trajectory: The masked trajectory.
    """
    traj_self = self if inplace else self.copy()

    traj_self.timestamps = traj_self.timestamps[mask]
    traj_self.positions.xyz = traj_self.positions.xyz[mask, :]

    if traj_self.rotations:
        quat_filtered = traj_self.rotations.as_quat()[mask, :]
        traj_self.rotations = Rotations.from_quat(quat_filtered) if len(quat_filtered) > 0 else None

    traj_self.path_lengths = traj_self.path_lengths[mask]

    if traj_self.velocity_xyz is not None:
        traj_self.velocity_xyz = traj_self.velocity_xyz[mask]

    return traj_self

transform

transform(
    transformation: ndarray, inplace: bool = True
) -> Trajectory

Applies a rigid body transformation to the trajectory poses.

Parameters:

• transformation (ndarray) –

  A 4x4 homogeneous transformation matrix.

• inplace (bool, default: True ) –

  If True, modifies self. Defaults to True.

Returns:

• Trajectory ( Trajectory ) –

  The transformed trajectory.

Source code in trajectopy\core\trajectory.py

def transform(self, transformation: np.ndarray, inplace: bool = True) -> "Trajectory":
    """
    Applies a rigid body transformation to the trajectory poses.

    Args:
        transformation (np.ndarray): A 4x4 homogeneous transformation matrix.
        inplace (bool, optional): If True, modifies self. Defaults to True.

    Returns:
        Trajectory: The transformed trajectory.
    """
    traj_self = self if inplace else self.copy()
    traj_self.se3 = [np.dot(transformation, p) for p in traj_self.se3]
    return traj_self

to_dataframe

to_dataframe(sort_by: str = '') -> DataFrame

Exports the trajectory to a Pandas DataFrame.

Columns usually include: time, path_length, pos_x, pos_y, pos_z, speed_x, speed_y, speed_z, and rotation columns (rot_x/y/z/w) if available.

Parameters:

• sort_by (str, default: '' ) –

  Column name to sort by. If empty, uses self.sorting.

Returns:

• DataFrame –

  pd.DataFrame: A dataframe containing the trajectory data.

Source code in trajectopy\core\trajectory.py

def to_dataframe(self, sort_by: str = "") -> pd.DataFrame:
    """
    Exports the trajectory to a Pandas DataFrame.

    Columns usually include: time, path_length, pos_x, pos_y, pos_z, speed_x, speed_y, speed_z,
    and rotation columns (rot_x/y/z/w) if available.

    Args:
        sort_by (str, optional): Column name to sort by. If empty, uses `self.sorting`.

    Returns:
        pd.DataFrame: A dataframe containing the trajectory data.
    """
    sort_by = sort_by or self.sorting
    if self.rotations:
        dataframe = pd.DataFrame(
            np.c_[
                self.timestamps, self.path_lengths, self.positions.xyz, self.rotations.as_quat(), self.velocity_xyz
            ],
            columns=[
                "time",
                "path_length",
                "pos_x",
                "pos_y",
                "pos_z",
                "rot_x",
                "rot_y",
                "rot_z",
                "rot_w",
                "speed_x",
                "speed_y",
                "speed_z",
            ],
        )
    else:
        dataframe = pd.DataFrame(
            np.c_[self.timestamps, self.path_lengths, self.positions.xyz, self.velocity_xyz],
            columns=["time", "path_length", "pos_x", "pos_y", "pos_z", "speed_x", "speed_y", "speed_z"],
        )

    return dataframe.sort_values(by=sort_by)

to_string

to_string() -> str

Serializes the trajectory to a CSV-formatted string with metadata headers.

Headers included: #epsg, #name, #nframe, #sorting, #fields.

Source code in trajectopy\core\trajectory.py

def to_string(self) -> str:
    """
    Serializes the trajectory to a CSV-formatted string with metadata headers.

    Headers included: #epsg, #name, #nframe, #sorting, #fields.
    """

    def write_header() -> str:
        fields = "t,l,px,py,pz,vx,vy,vz" if self.rotations is None else "t,l,px,py,pz,qx,qy,qz,qw,vx,vy,vz"
        header = [
            f"#epsg {self.positions.epsg}",
            f"#name {self.name}",
            "#nframe enu",
            f"#sorting {self.sorting.value}",
            f"#fields {fields}",
        ]
        return "\n".join(header) + "\n"

    if self.rotations is None:
        trajectory_data = np.c_[self.timestamps, self.path_lengths, self.positions.xyz, self.velocity_xyz]
    else:
        trajectory_data = np.c_[
            self.timestamps,
            self.path_lengths,
            self.positions.xyz,
            self.rotations.as_quat(),
            self.velocity_xyz,
        ]

    output = io.StringIO()
    output.write(write_header())
    pd.DataFrame(trajectory_data).to_csv(output, header=False, index=False, float_format="%.9f")

    return output.getvalue()

to_file

to_file(filename: str, mode: str = 'w') -> None

Writes the trajectory to an ASCII file using the format defined in to_string.

Parameters:

• filename (str) –

  The output file path.

• mode (str, default: 'w' ) –

  File open mode. Defaults to "w".

Source code in trajectopy\core\trajectory.py

def to_file(self, filename: str, mode: str = "w") -> None:
    """
    Writes the trajectory to an ASCII file using the format defined in `to_string`.

    Args:
        filename (str): The output file path.
        mode (str, optional): File open mode. Defaults to "w".
    """
    with open(filename, mode=mode, newline="\n", encoding="utf-8") as file:
        file.write(self.to_string())

to_kml

to_kml(filename: str, precision: float = 1e-06) -> str

Exports the trajectory to a Google Earth KML file.

Requires the trajectory to have a valid EPSG code so it can be converted to WGS84 (EPSG:4326).

Parameters:

• filename (str) –

  The output filename (e.g., "track.kml").

• precision (float, default: 1e-06 ) –

  Coordinate precision in degrees for rounding/simplification. Defaults to 1e-6.

Raises:

• ValueError –

  If the trajectory does not have a known EPSG code.

Source code in trajectopy\core\trajectory.py

def to_kml(self, filename: str, precision: float = 1e-6) -> str:
    """
    Exports the trajectory to a Google Earth KML file.

    Requires the trajectory to have a valid EPSG code so it can be converted to WGS84 (EPSG:4326).

    Args:
        filename (str): The output filename (e.g., "track.kml").
        precision (float, optional): Coordinate precision in degrees for rounding/simplification. Defaults to 1e-6.

    Raises:
        ValueError: If the trajectory does not have a known EPSG code.
    """
    traj = self.copy()
    if traj.positions.local_transformer is None:
        raise ValueError(
            "Trajectory must be defined in a well-known coordinate system (EPSG code) to be exported to KML. "
        )
    traj.positions.to_epsg(4326)

    traj.positions = traj.positions.round_to(precision)
    _, indices = np.unique(traj.positions.xyz[:, 0:2], return_index=True, axis=0)
    traj.mask(np.sort(indices))

    kml_file = ET.Element("kml", xmlns="http://earth.google.com/kml/2.1")
    document = ET.SubElement(kml_file, "Document")

    placemark = ET.SubElement(document, "Placemark")
    name = ET.SubElement(placemark, "name")
    name.text = traj.name

    style = ET.SubElement(placemark, "Style")
    line_style = ET.SubElement(style, "LineStyle")
    color = ET.SubElement(line_style, "color")
    color.text = "ff0000ff"
    width = ET.SubElement(line_style, "width")
    width.text = "2"

    line_string = ET.SubElement(placemark, "LineString")
    coordinates = ET.SubElement(line_string, "coordinates")

    coordinates.text = "\n".join(f"  {pos[1]:.9f},{pos[0]:.9f},{0.00:.3f}" for pos in traj.positions.xyz)

    tree = ET.ElementTree(kml_file)
    ET.indent(tree, space="", level=0)
    tree.write(filename, encoding="utf-8", xml_declaration=True)