Q. : Can you produce a sample code in Object Pascal to visualize the steps?

 

I will visualize the Extended Kalman Filter SLAM (EKF-SLAM) logic in Object Pascal syntax, using comments to explain the underlying math in programming terms.

The core idea is that SLAM is just a continuous cycle of Prediction (based on robot movement) and Update (based on LiDAR observations), all done with matrix algebra.

This example assumes you include a linear algebra unit, such as the mrMath library often used in Free Pascal projects, which provides the necessary TMatrix and TVector types and functions.

 

Object Pascal EKF-SLAM Structure

 

unit EKF_SLAM;

 

// --- Placeholder for a Matrix Library (e.g., using mrMath or a custom one) ---

// We define our core data types for the filter.

type

  // TVector and TMatrix must handle Double precision floats

  TVector = array of Double;  // State vector, Measurement vector

  TMatrix = array of TVector; // Covariance matrix, Jacobians, Kalman Gain

  

  // Your existing segment structure (Centroid is the landmark observation)

  TPoint = record

    X: Double;

    Y: Double;

  end;

  TSegment = record

    Centroid: TPoint; // The Landmark Measurement (z)

    Count: Integer;

    // ... other fields from your SLAM01.lpr ...

  end;

 

  // Record to hold the entire state of the SLAM system

  TSlamFilter = record

  private

    // **1. The State Vector (x_k):** Size (3 + 2*M)

    // x_k = [Robot_x, Robot_y, Robot_theta, M1_x, M1_y, M2_x, M2_y, ...]

    FStateVector: TVector;

 

    // **2. The Covariance Matrix (P_k):** Size (3 + 2*M) x (3 + 2*M)

    // Measures uncertainty and correlations between ALL states

    FCovarianceMatrix: TMatrix;

    

    // **System Parameters (Noise Matrices)**

    FProcessNoise_Q: TMatrix;      // Uncertainty in robot's motion (from odometry)

    FMeasurementNoise_R: TMatrix;  // Uncertainty in LiDAR measurements

    

    // Number of landmarks currently in the map

    FLandmarkCount: Integer;

    

    // Helper function to get the landmark position from the State Vector

    function GetLandmarkPos(Index: Integer): TPoint;

 

    // Helper functions for math (will be implemented using TMatrix/TVector library)

    function AddVectors(A, B: TVector): TVector;

    function MatrixMultiply(M1, M2: TMatrix): TMatrix;

    function MatrixTranspose(M: TMatrix): TMatrix;

    function MatrixInverse(M: TMatrix): TMatrix;

 

  public

    // Main SLAM loop functions

    procedure Initialize(InitialX, InitialY, InitialTheta: Double);

    procedure PredictionStep(DeltaT: Double; V_linear, W_angular: Double);

    procedure UpdateStep(const ObservedSegments: array of TSegment);

    procedure AugmentState(const NewSegment: TSegment);

    

    // ... other public methods like GetRobotPose, GetMapLandmarks ...

  end;

  

implementation

 

// ==============================================================================

// 1. STATE AUGMENTATION (New Landmark Initialization)

// ==============================================================================

procedure TSlamFilter.AugmentState(const NewSegment: TSegment);

var

  i: Integer;

  NewMapSize: Integer;

  // Temporary variables for the matrix math to calculate cross-correlations

  Gz, Fx: TMatrix;

  H_inv: TMatrix;

begin

  // A. Determine New State Size

  FLandmarkCount := FLandmarkCount + 1;

  NewMapSize := 3 + 2 * FLandmarkCount; // 3 for robot + 2 for each landmark

  

  // B. Resize State Vector (x_k)

  // Append the new landmark's global position to the end of FStateVector.

  // This involves converting the polar LiDAR measurement (range/bearing) 

  // into an absolute (X, Y) coordinate using the robot's current pose.

  

  // (Pseudo code for state augmentation)

  // x_new := Convert_LiDAR_to_Global(NewSegment.Centroid, FStateVector[1], FStateVector[3]);

  // Resize(FStateVector, NewMapSize);

  // FStateVector[NewMapSize - 1] := x_new.X;

  // FStateVector[NewMapSize] := x_new.Y;

 

  // C. Resize Covariance Matrix (P_k)

  // This is the most complex step: P_new = [ P_old  | P_rL

  //                                          P_Lr  | P_LL ]

  

  // (Pseudo code for covariance augmentation)

  // Fx := Jacobian_of_LiDAR_to_Global_wrt_Robot_Pose; // Gz in some literature

  // H_inv := Jacobian_of_LiDAR_to_Global_wrt_Measurement; // H_inv in some literature

  

  // P_rL (Robot-Landmark cross-correlation) = P_rr * Fx^T

  // P_LL (Landmark self-covariance) = Fx * P_rr * Fx^T + H_inv * R * H_inv^T 

  

  // Resize(FCovarianceMatrix, NewMapSize, NewMapSize);

  // ... fill the new rows/columns with the calculated P_rL, P_Lr, and P_LL ...

end;

 

// ==============================================================================

// 2. PREDICTION STEP (Robot Movement)

// ==============================================================================

procedure TSlamFilter.PredictionStep(DeltaT: Double; V_linear, W_angular: Double);

var

  // Jacobians are the matrices of partial derivatives

  Fx: TMatrix;  // Jacobian of Motion Model w.r.t. the State (x)

  Fu: TMatrix;  // Jacobian of Motion Model w.r.t. the Control Input (u)

  

  MotionInput: TVector; // [V_linear, W_angular]

  NewRobotPose: TVector; // Calculated from motion model

  

  // P_bar = F * P * F^T + Q

  F_P_F_T, F_P, Q_matrix: TMatrix;

  

begin

  // **PART 1: Predict the State Vector (x_k_bar)**

  // x_k_bar = f(x_{k-1}, u_k)

  

  // 1. Calculate the New Robot Pose

  // This is a non-linear function (f) of the previous pose and the odometry input.

  // NewRobotPose := MotionModel_f(FStateVector, V_linear, W_angular, DeltaT);

  

  // 2. Update the robot part of the state vector

  // The landmark positions remain unchanged in the prediction step.

  // FStateVector[1..3] := NewRobotPose;

  

  // **PART 2: Predict the Covariance Matrix (P_k_bar)**

  // P_k_bar = Fx * P_{k-1} * Fx^T + Q_k

  

  // 1. Calculate the State Jacobian (Fx)

  // Fx is the matrix showing how changes in the old state affect the new state.

  // Since only the robot moves, Fx is mostly an Identity matrix.

  // Fx := CalculateMotionJacobian_Fx(FStateVector, MotionInput);

  

  // 2. Calculate the Motion Noise Matrix (Q_k)

  // This is based on the input noise (e.g., wheel slippage) and the input Jacobian.

  // Q_matrix := Fu * FProcessNoise_Q * Fu^T;

  // This is a full-size sparse matrix (3+2M x 3+2M) with values only in the 3x3 robot part.

  

  // 3. Apply the EKF Prediction Equation

  // F_P := MatrixMultiply(Fx, FCovarianceMatrix);

  // F_P_F_T := MatrixMultiply(F_P, MatrixTranspose(Fx));

  // FCovarianceMatrix := AddMatrices(F_P_F_T, Q_matrix); // P_k_bar

end;

 

// ==============================================================================

// 3. UPDATE STEP (LiDAR Correction)

// ==============================================================================

procedure TSlamFilter.UpdateStep(const ObservedSegments: array of TSegment);

var

  i: Integer;

  // Core EKF matrices for the update step

  PredictedMeasurement_z_hat: TVector;

  MeasurementJacobian_H: TMatrix;

  InnovationCovariance_S: TMatrix; // H * P_bar * H^T + R

  KalmanGain_K: TMatrix;           // P_bar * H^T * S^{-1}

  Innovation_y: TVector;           // z - z_hat

begin

  // Loop through every segment centroid observed by the LiDAR

  for i := Low(ObservedSegments) to High(ObservedSegments) do

  begin

    // **PART 1: Data Association (Find the right landmark in the map)**

    // This step finds the best match for ObservedSegments[i].Centroid 

    // among all the landmarks (M1, M2, ...) currently in FStateVector.

    

    // MatchedIndex := Find_Best_Match_in_Map(ObservedSegments[i].Centroid, FStateVector);

    

    // If a match (MatchedIndex > 0) is found: Proceed to Correction

    // else: Call AugmentState for a new landmark (Part 1 logic)

    

    // **PART 2: Correction/Update** (Assume MatchedIndex is the landmark Mj)

    

    // 1. Calculate Predicted Measurement (z_hat)

    // z_hat = h(x_bar) : The range and bearing the robot EXPECTS to see 

    // to the matched landmark, based on the predicted robot pose (x_bar).

    // PredictedMeasurement_z_hat := ObservationModel_h(FStateVector, MatchedIndex);

 

    // 2. Calculate Measurement Jacobian (H)

    // H is the matrix showing how a change in the state (x) affects the measurement (z).

    // It is a sparse matrix: non-zero values only link the measurement to the 

    // robot's pose and the matched landmark's position.

    // MeasurementJacobian_H := CalculateObservationJacobian_H(FStateVector, MatchedIndex);

    

    // 3. Calculate Innovation Covariance (S)

    // S = H * P_bar * H^T + R

    // R is the noise in your LiDAR sensor.

    // S_temp := MatrixMultiply(MeasurementJacobian_H, FCovarianceMatrix);

    // InnovationCovariance_S := AddMatrices(MatrixMultiply(S_temp, MatrixTranspose(MeasurementJacobian_H)), FMeasurementNoise_R);

    

    // 4. Calculate Kalman Gain (K)

    // K = P_bar * H^T * S^{-1}

    // KalmanGain_K := MatrixMultiply(MatrixMultiply(FCovarianceMatrix, MatrixTranspose(MeasurementJacobian_H)), MatrixInverse(InnovationCovariance_S));

    

    // 5. Calculate Innovation (y)

    // y = z - z_hat

    // This is the difference between what the LiDAR SAW (z) and what the EKF EXPECTED (z_hat).

    // Innovation_y := SubtractVectors(ObservedSegments[i].Centroid, PredictedMeasurement_z_hat);

    

    // 6. Update the State Vector (x_k)

    // x_k = x_bar + K * y

    // FStateVector := AddVectors(FStateVector, VectorMultiply(KalmanGain_K, Innovation_y));

    

    // 7. Update the Covariance Matrix (P_k)

    // P_k = (I - K * H) * P_bar

    // P_temp := SubtractMatrices(IdentityMatrix, MatrixMultiply(KalmanGain_K, MeasurementJacobian_H));

    // FCovarianceMatrix := MatrixMultiply(P_temp, FCovarianceMatrix);

  end;

end;

 

end.