diff --git a/examples/dave_demos/launch/lidar_3d_demo.launch.py b/examples/dave_demos/launch/lidar_3d_demo.launch.py
new file mode 100644
index 00000000..97326e75
--- /dev/null
+++ b/examples/dave_demos/launch/lidar_3d_demo.launch.py
@@ -0,0 +1,70 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    # 1. Paths to your new assets
+    pkg_dave_worlds = get_package_share_directory('dave_worlds')
+    pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
+    pkg_dave_demos = get_package_share_directory('dave_demos')
+
+    # Path to your new world file
+    world_path = os.path.join(pkg_dave_worlds, 'worlds', 'lidar_3d.world')
+
+    # 2. Launch Gazebo with the new world
+    # We include the standard Gazebo Sim launch but pass our custom world
+    gz_sim = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_dave_demos, 'launch', "dave_sensor.launch.py")
+        ),
+        launch_arguments={
+            "namespace": "lidar_3d",
+            "world_name": "lidar_3d",
+            "paused": "false",
+            "debug": "true",
+            "x": "5.8",
+            "z": "2",
+            "yaw": "3.14",
+            'gz_args': f'-r {world_path} --render-engine ogre2'
+        }.items(),
+    )
+
+    # This replaces the DAVE multibeam bridge with one for your 3D LiDAR
+    bridge = Node(
+        package='ros_gz_bridge',
+        executable='parameter_bridge',
+        arguments=[
+            # LiDAR LaserScan
+            '/lidar@sensor_msgs/msg/LaserScan[gz.msgs.LaserScan',
+            # LiDAR PointCloud
+            '/lidar/points@sensor_msgs/msg/PointCloud2[gz.msgs.PointCloudPacked',
+            # Clock bridge (essential for TF and sensor timing)
+            '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'
+        ],
+        remappings=[
+        ('/lidar', '/lidar_3d/lidar'),
+        ('/lidar/points', '/lidar_3d/lidar/points')
+        ],
+        output='screen'
+    )
+
+    # Connects the sensor frame to the world frame for RViz visualization
+    tf_node = Node(
+    package='tf2_ros',
+    executable='static_transform_publisher',
+    arguments=[
+        '--x', '0', '--y', '0', '--z', '0.5',
+        '--roll', '0', '--pitch', '0', '--yaw', '0',
+        '--frame-id', 'world',
+        '--child-frame-id', 'lidar_3d/lidar_3d_base_link/gpu_lidar'
+    ],
+)
+
+    return LaunchDescription([
+        gz_sim,
+        bridge,
+        tf_node
+    ])
\ No newline at end of file
diff --git a/gazebo/dave_gz_multibeam_sonar/multibeam_sonar/sonar_calculation_cuda.cu b/gazebo/dave_gz_multibeam_sonar/multibeam_sonar/sonar_calculation_cuda.cu
index f01da386..85033f9f 100644
--- a/gazebo/dave_gz_multibeam_sonar/multibeam_sonar/sonar_calculation_cuda.cu
+++ b/gazebo/dave_gz_multibeam_sonar/multibeam_sonar/sonar_calculation_cuda.cu
@@ -144,6 +144,25 @@ __device__ __host__ float unnormalized_sinc(float t)
   }
 }
 
+///////////////////////////////////////////////////////////////////////////
+// 3D Incident Angle Calculation
+__device__ float compute_incidence_3d(float azimuth, float elevation, float * normal)
+{
+  // Convert spherical ray coordinates to 3D cartesian vector
+  float ray_x = cosf(azimuth) * cosf(elevation);
+  float ray_y = sinf(azimuth) * cosf(elevation);
+  float ray_z = sinf(elevation);
+
+  // Align Gazebo target normal to camera axes
+  float target_normal[3] = {normal[2], -normal[0], -normal[1]};
+
+  // 3D Dot product for volumetric incidence
+  float dot_product = ray_x * target_normal[0] + ray_y * target_normal[1] + ray_z * target_normal[2];
+  
+  // Clamp to prevent acosf NaN errors
+  return acosf(fmaxf(-1.0f, fminf(1.0f, dot_product)));
+}
+
 __global__ void reduce_beams_kernel(
   const thrust::complex<float> * __restrict__ d_P_Beams, float * d_P_Beams_Cor_real,
   float * d_P_Beams_Cor_imag, int nBeams, int nFreq, int nRaysSkipped)
@@ -295,6 +314,98 @@ __global__ void sonar_calculation(
   }
 }
 
+///////////////////////////////////////////////////////////////////////////
+// 3D Sonar Volumetric Kernel
+__global__ void sonar_calculation_3d(
+  thrust::complex<float> * P_Beams, float * depth_image, float * normal_image, 
+  int width, int height, int depth_image_step, int normal_image_step, unsigned long long seed,
+  float * reflectivity_image, int reflectivity_image_step, 
+  float hFOV, float vFOV, float soundSpeed, float sourceTerm, 
+  int nBeams_h, int nBeams_v, int raySkips, float delta_f, int nFreq, 
+  float maxDistance, float attenuation, float area_scaler)
+{
+  // Volumetric mapping: x = azimuth/horizontal, y = elevation/vertical
+  const int x_idx = blockIdx.x * blockDim.x + threadIdx.x;
+  const int y_idx = blockIdx.y * blockDim.y + threadIdx.y;
+
+  if (x_idx < width && y_idx < height && (y_idx % raySkips == 0) && (x_idx % raySkips == 0))
+  {
+    const int depth_idx = y_idx * (depth_image_step / sizeof(float)) + x_idx;
+    const int norm_idx = y_idx * (normal_image_step / sizeof(float)) + (3 * x_idx);
+    const int refl_idx = y_idx * (reflectivity_image_step / sizeof(float)) + x_idx;
+
+    float distance = depth_image[depth_idx];
+    if (distance <= 0.001f || distance > maxDistance) return;
+
+    float normal[3] = {normal_image[norm_idx], normal_image[norm_idx+1], normal_image[norm_idx+2]};
+
+    // Calculate specific azimuth and elevation for this 3D ray
+    float azimuth = (x_idx / (float)width - 0.5f) * hFOV;
+    float elevation = (y_idx / (float)height - 0.5f) * vFOV;
+
+    float incidence = compute_incidence_3d(azimuth, elevation, normal);
+
+    // Speckle noise generation
+    curandStatePhilox4_32_10_t state;
+    curand_init(seed, y_idx * width + x_idx, 0, &state);
+    float4 xi = curand_normal4(&state);
+    
+    thrust::complex<float> randomAmps(xi.x / sqrtf(2.0f), xi.y / sqrtf(2.0f));
+    float lambert = cosf(incidence);
+    float p_loss = (1.0f / (distance * distance)) * expf(-2.0f * attenuation * distance);
+    float amplitude_scalar = sourceTerm * p_loss * lambert * sqrtf(reflectivity_image[refl_idx] * area_scaler);
+
+    thrust::complex<float> base_amplitude = randomAmps * thrust::complex<float>(amplitude_scalar, 0.0f);
+
+    // Map 3D ray to specific voxel in the 3D Beam Matrix
+    int beam_h = (x_idx * nBeams_h) / width;
+    int beam_v = (y_idx * nBeams_v) / height;
+    int beam_3d_idx = beam_v * nBeams_h + beam_h;
+
+    for (int f = 0; f < nFreq; f++)
+    {
+      float freq = delta_f * (f - nFreq / 2.0f); // Centered frequency
+      float kw = (2.0f * M_PI * freq) / soundSpeed;
+      float phase = 2.0f * distance * kw;
+
+      float s, c;
+      __sincosf(phase, &s, &c);
+      thrust::complex<float> kernel = thrust::complex<float>(c, s) * base_amplitude;
+
+      // Because multiple rays map to one voxel, we must use atomicAdd to sum interference
+      // casting complex to float2 for atomic addition
+      float2* p_beams_ptr = (float2*)&P_Beams[beam_3d_idx * nFreq + f];
+      atomicAdd(&(p_beams_ptr->x), kernel.real());
+      atomicAdd(&(p_beams_ptr->y), kernel.imag());
+    }
+  }
+}
+
+///////////////////////////////////////////////////////////////////////////
+// 3D Reduction Kernel
+__global__ void reduce_beams_3d_kernel(
+  const thrust::complex<float> * __restrict__ d_P_Beams, 
+  float * d_P_Beams_Cor_real, float * d_P_Beams_Cor_imag, 
+  int nBeams_h, int nBeams_v, int nFreq)
+{
+  // Maps to a 3D Grid: x = Frequency Bin, y = Horizontal Beam, z = Vertical Beam
+  int f = blockIdx.x;
+  int beam_h = blockIdx.y;
+  int beam_v = blockIdx.z;
+
+  if (f >= nFreq || beam_h >= nBeams_h || beam_v >= nBeams_v) return;
+
+  int voxel_idx = (beam_v * nBeams_h + beam_h) * nFreq + f;
+  
+  // Extract and apply reduction mappings to final arrays 
+  thrust::complex<float> val = d_P_Beams[voxel_idx];
+  
+  // Output flattened for CUBLAS/CUFFT processing
+  int out_idx = f * (nBeams_h * nBeams_v) + (beam_v * nBeams_h + beam_h);
+  d_P_Beams_Cor_real[out_idx] = val.real();
+  d_P_Beams_Cor_imag[out_idx] = val.imag();
+}
+
 ///////////////////////////////////////////////////////////////////////////
 namespace NpsGazeboSonar
 {
@@ -743,4 +854,256 @@ CArray2D sonar_calculation_wrapper(
 
   return P_Beams_F;
 }
+
+// sonar calculation 3D wrapper
+CArray3D sonar_calculation_3d_wrapper(
+  const cv::Mat & depth_image, const cv::Mat & normal_image, double _hPixelSize, double _vPixelSize,
+  double _hFOV, double _vFOV, double _beam_azimuthAngleWidth, double _beam_elevationAngleWidth,
+  double _ray_azimuthAngleWidth, float * _ray_elevationAngles, double _ray_elevationAngleWidth,
+  double _soundSpeed, double _maxDistance, double _sourceLevel, 
+  int _nBeams_h, int _nBeams_v, int _nRays_h, int _nRays_v,
+  int _raySkips, double _sonarFreq, double _bandwidth, int _nFreq,
+  const cv::Mat & reflectivity_image, double _attenuation, float * window, 
+  float ** beamCorrector, float beamCorrectorSum, bool debugFlag, bool blazingFlag)
+{
+    // Convert and cast parameters
+    const float hFOV = (float)_hFOV;
+    const float vFOV = (float)_vFOV;
+    const float soundSpeed = (float)_soundSpeed;
+    const float maxDistance = (float)_maxDistance;
+    const float attenuation = (float)_attenuation;
+    const float bandwidth = (float)_bandwidth;
+    const float sonarFreq = (float)_sonarFreq;
+    const int nFreq = _nFreq;
+    const int raySkips = _raySkips;
+    const int nRays_h = _nRays_h;
+    const int nRays_v = _nRays_v;
+    const int nBeams_h = _nBeams_h;
+    const int nBeams_v = _nBeams_v;
+
+    const int total_beams = nBeams_h * nBeams_v;
+
+    // Prepare output container
+    CArray3D P_Beams_3D(CArray2D(CArray(_nFreq), nBeams_h), nBeams_v);
+
+    // FFT params
+    const int DATASIZE = nFreq;
+    const int BATCH = total_beams;
+    const float delta_f = bandwidth / (float)nFreq;
+
+    // Allocate temporary device arrays for 3D beam accumulation
+    thrust::complex<float> * d_P_Beams_3d = nullptr;
+    SAFE_CALL(
+      cudaMalloc((void **)&d_P_Beams_3d, sizeof(thrust::complex<float>) * BATCH * DATASIZE),
+      "cudaMalloc Failed for d_P_Beams_3d");
+    SAFE_CALL(cudaMemset(d_P_Beams_3d, 0, sizeof(thrust::complex<float>) * BATCH * DATASIZE),
+              "cudaMemset Failed for d_P_Beams_3d");
+
+    // Copy ray angles if needed (reuse existing host array)
+    for (int ray = 0; ray < nRays_h * nRays_v; ++ray)
+    {
+      if (ray < nRays_h * nRays_v) ray_elevationAngles[ray] = _ray_elevationAngles[ray];
+    }
+
+    // Launch sonar_calculation_3d kernel over the image (azimuth x elevation)
+    const dim3 block(BLOCK_SIZE, BLOCK_SIZE);
+    const dim3 grid((depth_image.cols + block.x - 1) / block.x,
+                    (depth_image.rows + block.y - 1) / block.y);
+
+    unsigned long long seed = blazingFlag ? static_cast<unsigned long long>(time(NULL)) : 1234ULL;
+
+    sonar_calculation_3d<<<grid, block>>>(
+      d_P_Beams_3d, (float *)d_depth_image, (float *)d_normal_image, depth_image.cols,
+      depth_image.rows, depth_image.step, normal_image.step, seed, (float *)d_reflectivity_image,
+      reflectivity_image.step, hFOV, vFOV, soundSpeed, (float)sqrt(pow(10, (_sourceLevel / 10))) * 1e-6f,
+      nBeams_h, nBeams_v, raySkips, delta_f, nFreq, maxDistance, attenuation,
+      (float)(_ray_azimuthAngleWidth * _ray_elevationAngleWidth));
+
+    SAFE_CALL(cudaGetLastError(), "sonar_calculation_3d launch failed");
+    SAFE_CALL(cudaDeviceSynchronize(), "sonar_calculation_3d execution failed");
+
+    // Reduce/permute GPU buffer into freq-major arrays suitable for GEMM/FFT
+    float * d_P_Beams_Cor_real_local = nullptr;
+    float * d_P_Beams_Cor_imag_local = nullptr;
+    SAFE_CALL(cudaMalloc((void **)&d_P_Beams_Cor_real_local, sizeof(float) * BATCH * DATASIZE),
+              "cudaMalloc Failed for d_P_Beams_Cor_real_local");
+    SAFE_CALL(cudaMalloc((void **)&d_P_Beams_Cor_imag_local, sizeof(float) * BATCH * DATASIZE),
+              "cudaMalloc Failed for d_P_Beams_Cor_imag_local");
+
+    dim3 redGrid(nFreq, nBeams_h, nBeams_v);
+    reduce_beams_3d_kernel<<<redGrid, dim3(1)>>>(
+      d_P_Beams_3d, d_P_Beams_Cor_real_local, d_P_Beams_Cor_imag_local, nBeams_h, nBeams_v,
+      nFreq);
+    SAFE_CALL(cudaGetLastError(), "reduce_beams_3d_kernel launch failed");
+    SAFE_CALL(cudaDeviceSynchronize(), "reduce_beams_3d_kernel execution failed");
+
+    // Host-side buffers for reduced data
+    float * P_Beams_Cor_real_h_local = nullptr;
+    float * P_Beams_Cor_imag_h_local = nullptr;
+    SAFE_CALL(cudaMallocHost((void **)&P_Beams_Cor_real_h_local, sizeof(float) * BATCH * DATASIZE),
+              "cudaMallocHost Failed for P_Beams_Cor_real_h_local");
+    SAFE_CALL(cudaMallocHost((void **)&P_Beams_Cor_imag_h_local, sizeof(float) * BATCH * DATASIZE),
+              "cudaMallocHost Failed for P_Beams_Cor_imag_h_local");
+
+    SAFE_CALL(cudaMemcpy(P_Beams_Cor_real_h_local, d_P_Beams_Cor_real_local,
+                         sizeof(float) * BATCH * DATASIZE, cudaMemcpyDeviceToHost),
+              "CUDA Memcpy Failed for P_Beams_Cor_real_h_local");
+    SAFE_CALL(cudaMemcpy(P_Beams_Cor_imag_h_local, d_P_Beams_Cor_imag_local,
+                         sizeof(float) * BATCH * DATASIZE, cudaMemcpyDeviceToHost),
+              "CUDA Memcpy Failed for P_Beams_Cor_imag_h_local");
+
+    // Build linearized beamCorrector matrix (host + device)
+    float * beamCorrector_lin_h_local = nullptr;
+    float * d_beamCorrector_lin_local = nullptr;
+    const size_t beamCorrector_lin_bytes_local = sizeof(float) * (size_t)BATCH * (size_t)BATCH;
+    SAFE_CALL(cudaMallocHost((void **)&beamCorrector_lin_h_local, beamCorrector_lin_bytes_local),
+              "cudaMallocHost Failed for beamCorrector_lin_h_local");
+    for (int b = 0; b < total_beams; ++b)
+    {
+      for (int bo = 0; bo < total_beams; ++bo)
+      {
+        beamCorrector_lin_h_local[bo * total_beams + b] = beamCorrector[b][bo];
+      }
+    }
+    SAFE_CALL(cudaMalloc((void **)&d_beamCorrector_lin_local, beamCorrector_lin_bytes_local),
+              "cudaMalloc Failed for d_beamCorrector_lin_local");
+    SAFE_CALL(cudaMemcpy(d_beamCorrector_lin_local, beamCorrector_lin_h_local, beamCorrector_lin_bytes_local,
+                         cudaMemcpyHostToDevice),
+              "CUDA Memcpy Failed for d_beamCorrector_lin_local");
+
+    // Prepare device outputs for GEMM
+    float * d_P_Beams_Cor_F_real_local = nullptr;
+    float * d_P_Beams_Cor_F_imag_local = nullptr;
+    SAFE_CALL(cudaMalloc((void **)&d_P_Beams_Cor_F_real_local, sizeof(float) * BATCH * DATASIZE),
+              "cudaMalloc Failed for d_P_Beams_Cor_F_real_local");
+    SAFE_CALL(cudaMalloc((void **)&d_P_Beams_Cor_F_imag_local, sizeof(float) * BATCH * DATASIZE),
+              "cudaMalloc Failed for d_P_Beams_Cor_F_imag_local");
+
+    // cuBLAS GEMM: apply beamCorrector to each frequency slice
+    cublasHandle_t cublas_handle;
+    SAFE_CUBLAS_CALL(cublasCreate(&cublas_handle), "cublasCreate Failed");
+
+    const int M_gemm = nFreq;
+    const int N_gemm = total_beams;
+    const int K_gemm = total_beams;
+    const float alpha = 1.0f;
+    const float beta = 0.0f;
+
+    // Real part
+    SAFE_CUBLAS_CALL(
+      cublasSgemm(cublas_handle, CUBLAS_OP_N, CUBLAS_OP_N, N_gemm, M_gemm, K_gemm, &alpha,
+                  d_beamCorrector_lin_local, N_gemm, d_P_Beams_Cor_real_local, K_gemm, &beta,
+                  d_P_Beams_Cor_F_real_local, N_gemm),
+      "cublasSgemm Failed for real part (3D)");
+
+    // Imag part
+    SAFE_CUBLAS_CALL(
+      cublasSgemm(cublas_handle, CUBLAS_OP_N, CUBLAS_OP_N, N_gemm, M_gemm, K_gemm, &alpha,
+                  d_beamCorrector_lin_local, N_gemm, d_P_Beams_Cor_imag_local, K_gemm, &beta,
+                  d_P_Beams_Cor_F_imag_local, N_gemm),
+      "cublasSgemm Failed for imag part (3D)");
+
+    SAFE_CUBLAS_CALL(cublasDestroy_v2(cublas_handle), "cublasDestroy Failed");
+
+    // Copy GEMM results back to host reduced buffers
+    float * P_Beams_Cor_real_h_final = nullptr;
+    float * P_Beams_Cor_imag_h_final = nullptr;
+    SAFE_CALL(cudaMallocHost((void **)&P_Beams_Cor_real_h_final, sizeof(float) * BATCH * DATASIZE),
+              "cudaMallocHost Failed for P_Beams_Cor_real_h_final");
+    SAFE_CALL(cudaMallocHost((void **)&P_Beams_Cor_imag_h_final, sizeof(float) * BATCH * DATASIZE),
+              "cudaMallocHost Failed for P_Beams_Cor_imag_h_final");
+
+    SAFE_CALL(cudaMemcpy(P_Beams_Cor_real_h_final, d_P_Beams_Cor_F_real_local,
+                         sizeof(float) * BATCH * DATASIZE, cudaMemcpyDeviceToHost),
+              "CUDA Memcpy Failed for P_Beams_Cor_real_h_final");
+    SAFE_CALL(cudaMemcpy(P_Beams_Cor_imag_h_final, d_P_Beams_Cor_F_imag_local,
+                         sizeof(float) * BATCH * DATASIZE, cudaMemcpyDeviceToHost),
+              "CUDA Memcpy Failed for P_Beams_Cor_imag_h_final");
+
+    // --- Prepare hostInputData for FFT (beam-major ordering)
+    cufftComplex * hostInputData = (cufftComplex *)malloc(DATASIZE * BATCH * sizeof(cufftComplex));
+
+#pragma omp parallel for collapse(2)
+    for (int beam = 0; beam < BATCH; ++beam)
+    {
+      for (int f = 0; f < DATASIZE; ++f)
+      {
+        int idx = beam * DATASIZE + f;
+        // Note reduced arrays are freq-major: [f * BATCH + beam]
+        hostInputData[idx] = make_cuComplex(
+          P_Beams_Cor_real_h_final[f * BATCH + beam] / beamCorrectorSum,
+          P_Beams_Cor_imag_h_final[f * BATCH + beam] / beamCorrectorSum);
+      }
+    }
+
+    // Device input/output for FFT
+    cufftComplex * deviceInputData_local = nullptr;
+    cufftComplex * deviceOutputData_local = nullptr;
+    SAFE_CALL(cudaMalloc((void **)&deviceInputData_local, DATASIZE * BATCH * sizeof(cufftComplex)),
+              "FFT cudaMalloc Failed for deviceInputData_local");
+    SAFE_CALL(cudaMalloc((void **)&deviceOutputData_local, DATASIZE * BATCH * sizeof(cufftComplex)),
+              "FFT cudaMalloc Failed for deviceOutputData_local");
+
+    SAFE_CALL(cudaMemcpy(deviceInputData_local, hostInputData, DATASIZE * BATCH * sizeof(cufftComplex),
+                         cudaMemcpyHostToDevice),
+              "FFT CUDA Memcopy Failed");
+
+    cufftComplex * hostOutputData = (cufftComplex *)malloc(DATASIZE * BATCH * sizeof(cufftComplex));
+
+    // --- Batched 1D FFTs
+    cufftHandle handle;
+    int rank = 1;
+    int n[] = {DATASIZE};
+    int istride = 1, ostride = 1;
+    int idist = DATASIZE, odist = DATASIZE;
+    int inembed[] = {0};
+    int onembed[] = {0};
+    int batch = BATCH;
+    SAFE_CUFFT_CALL(cufftPlanMany(&handle, rank, n, inembed, istride, idist, onembed, ostride, odist,
+                                  CUFFT_C2C, batch),
+                   "cufftPlanMany Failed");
+
+    SAFE_CUFFT_CALL(cufftExecC2C(handle, deviceInputData_local, deviceOutputData_local, CUFFT_FORWARD),
+                    "cufftExecC2C Failed");
+
+    SAFE_CALL(cudaMemcpy(hostOutputData, deviceOutputData_local, DATASIZE * BATCH * sizeof(cufftComplex),
+                         cudaMemcpyDeviceToHost),
+              "FFT CUDA Memcopy Failed");
+
+    // Pack results into P_Beams_3D: indexing beam = beam_v * nBeams_h + beam_h
+    for (int beam_v = 0; beam_v < nBeams_v; ++beam_v)
+    {
+      for (int beam_h = 0; beam_h < nBeams_h; ++beam_h)
+      {
+        int beam = beam_v * nBeams_h + beam_h;
+        for (int f = 0; f < nFreq; ++f)
+        {
+          int idx = beam * DATASIZE + f;
+          P_Beams_3D[beam_v][beam_h][f] = Complex(hostOutputData[idx].x * delta_f,
+                                                 hostOutputData[idx].y * delta_f);
+        }
+      }
+    }
+
+    // Cleanup temporaries
+    cufftDestroy(handle);
+    free(hostInputData);
+    free(hostOutputData);
+    SAFE_CALL(cudaFree(deviceInputData_local), "cudaFree failed for deviceInputData_local");
+    SAFE_CALL(cudaFree(deviceOutputData_local), "cudaFree failed for deviceOutputData_local");
+    SAFE_CALL(cudaFree(d_P_Beams_3d), "cudaFree failed for d_P_Beams_3d");
+    SAFE_CALL(cudaFree(d_P_Beams_Cor_real_local), "cudaFree failed for d_P_Beams_Cor_real_local");
+    SAFE_CALL(cudaFree(d_P_Beams_Cor_imag_local), "cudaFree failed for d_P_Beams_Cor_imag_local");
+    SAFE_CALL(cudaFree(d_P_Beams_Cor_F_real_local), "cudaFree failed for d_P_Beams_Cor_F_real_local");
+    SAFE_CALL(cudaFree(d_P_Beams_Cor_F_imag_local), "cudaFree failed for d_P_Beams_Cor_F_imag_local");
+    SAFE_CALL(cudaFree(d_beamCorrector_lin_local), "cudaFree failed for d_beamCorrector_lin_local");
+    SAFE_CALL(cudaFreeHost(P_Beams_Cor_real_h_local), "cudaFreeHost failed for P_Beams_Cor_real_h_local");
+    SAFE_CALL(cudaFreeHost(P_Beams_Cor_imag_h_local), "cudaFreeHost failed for P_Beams_Cor_imag_h_local");
+    SAFE_CALL(cudaFreeHost(beamCorrector_lin_h_local), "cudaFreeHost failed for beamCorrector_lin_h_local");
+    SAFE_CALL(cudaFreeHost(P_Beams_Cor_real_h_final), "cudaFreeHost failed for P_Beams_Cor_real_h_final");
+    SAFE_CALL(cudaFreeHost(P_Beams_Cor_imag_h_final), "cudaFreeHost failed for P_Beams_Cor_imag_h_final");
+
+    return P_Beams_3D;
+}
+
 }  // namespace NpsGazeboSonar
\ No newline at end of file
diff --git a/gazebo/dave_gz_multibeam_sonar/multibeam_sonar/sonar_calculation_cuda.cuh b/gazebo/dave_gz_multibeam_sonar/multibeam_sonar/sonar_calculation_cuda.cuh
index 651c05da..f5539067 100644
--- a/gazebo/dave_gz_multibeam_sonar/multibeam_sonar/sonar_calculation_cuda.cuh
+++ b/gazebo/dave_gz_multibeam_sonar/multibeam_sonar/sonar_calculation_cuda.cuh
@@ -36,6 +36,7 @@ namespace NpsGazeboSonar
 typedef std::complex<float> Complex;
 typedef std::valarray<Complex> CArray;
 typedef std::valarray<CArray> CArray2D;
+typedef std::valarray<CArray2D> CArray3D;
 
 /// \brief CUDA Device Check Function Wrapper
 void check_cuda_init_wrapper(void);
@@ -52,4 +53,19 @@ CArray2D sonar_calculation_wrapper(
   int _raySkips, double _sonarFreq, double _bandwidth, int _nFreq,
   const cv::Mat & reflectivity_image, double _attenuation, float * _window, float ** _beamCorrector,
   float _beamCorrectorSum, bool _debugFlag, bool _blazingFlag);
+
+/// \brief Sonar 3D Volumetric Calculation Function Wrapper
+/// \param _nBeams_v Vertical beam count for 3D frustum
+/// \param _vFOV Vertical Field of View
+CArray3D sonar_calculation_3d_wrapper(
+  const cv::Mat & depth_image, const cv::Mat & normal_image, double _hPixelSize, double _vPixelSize,
+  double _hFOV, double _vFOV, double _beam_azimuthAngleWidth, double _beam_elevationAngleWidth,
+  double _ray_azimuthAngleWidth, float * _ray_elevationAngles, double _ray_elevationAngleWidth,
+  double _soundSpeed, double _maxDistance, double _sourceLevel, 
+  int _nBeams_h, int _nBeams_v, // 3D dimensions
+  int _nRays_h, int _nRays_v,
+  int _raySkips, double _sonarFreq, double _bandwidth, int _nFreq,
+  const cv::Mat & reflectivity_image, double _attenuation, float * window, 
+  float ** beamCorrector, float beamCorrectorSum, bool debugFlag, bool blazingFlag);
+
 }  // namespace NpsGazeboSonar
\ No newline at end of file
diff --git a/models/dave_sensor_models/description/lidar_3d/model.config b/models/dave_sensor_models/description/lidar_3d/model.config
new file mode 100644
index 00000000..cf42e985
--- /dev/null
+++ b/models/dave_sensor_models/description/lidar_3d/model.config
@@ -0,0 +1,10 @@
+<?xml version="1.0"?>
+<model>
+  <name>lidar_3d</name>
+  <version>1.0</version>
+  <sdf version="1.6">model.sdf</sdf>
+
+  <description>
+    Lidar_3d
+  </description>
+</model>
\ No newline at end of file
diff --git a/models/dave_sensor_models/description/lidar_3d/model.sdf b/models/dave_sensor_models/description/lidar_3d/model.sdf
new file mode 100644
index 00000000..8ad37ea5
--- /dev/null
+++ b/models/dave_sensor_models/description/lidar_3d/model.sdf
@@ -0,0 +1,116 @@
+<?xml version="1.0"?>
+<sdf version="1.7">
+  <model name="lidar_3d">
+    <pose>4 0 0.5 0 0.0 3.14</pose>
+    <link name="lidar_3d_base_link">
+      <inertial>
+        <pose>0 0 0 0 0 0</pose>
+        <mass>3.5</mass>
+        <inertia>
+          <ixx>0.0195872</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.0195872</iyy>
+          <iyz>0</iyz>
+          <izz>0.0151357</izz>
+        </inertia>
+      </inertial>
+      <gravity>1</gravity>
+
+      <!-- Camera sensor -->
+      <sensor name="camera" type="camera">
+        <camera>
+          <horizontal_fov>1.047</horizontal_fov>
+          <image>
+            <width>320</width>
+            <height>240</height>
+          </image>
+          <clip>
+            <near>0.1</near>
+            <far>100</far>
+          </clip>
+        </camera>
+        <always_on>1</always_on>
+        <update_rate>30</update_rate>
+        <visualize>true</visualize>
+        <topic>/sensor/camera</topic>
+      </sensor>
+
+      <!-- Depth Camera sensor -->
+      <sensor name="depth_camera" type="depth_camera">
+        <update_rate>10</update_rate>
+        <topic>/sensor/depth_camera</topic>
+        <enable_metrics>true</enable_metrics>
+        <camera>
+          <horizontal_fov>1.05</horizontal_fov>
+          <image>
+            <width>320</width>
+            <height>240</height>
+            <format>R_FLOAT32</format>
+          </image>
+          <clip>
+            <near>0.1</near>
+            <far>10.0</far>
+          </clip>
+        </camera>
+      </sensor>
+
+      <!-- Replace Multibeam sonar with 3D lidar instead -->
+      <sensor name="gpu_lidar" type="gpu_lidar">
+        <pose>0 0 0 0 0 0</pose>
+        <update_rate>10</update_rate>
+        <lidar>
+          <scan>
+            <horizontal>
+              <samples>640</samples>
+              <resolution>1</resolution>
+              <min_angle>-3.14159</min_angle>
+              <max_angle>3.14159</max_angle>
+            </horizontal>
+            <vertical>
+              <samples>16</samples>
+              <resolution>1</resolution>
+              <min_angle>-0.261799</min_angle>
+              <max_angle>0.261799</max_angle>
+            </vertical>
+          </scan>
+          <range>
+            <min>0.1</min>
+            <max>30.0</max>
+            <resolution>0.01</resolution>
+          </range>
+        </lidar>
+        <always_on>1</always_on>
+        <visualize>true</visualize>
+        <topic>lidar</topic>
+      </sensor>
+
+
+      <self_collide>0</self_collide>
+      <enable_wind>0</enable_wind>
+      <kinematic>0</kinematic>
+
+      <visual name="lidar_3d_base_link_visual">
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://meshes/blueview_p900/p900.dae</uri>
+          </mesh>
+        </geometry>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name="lidar_3d_base_link_collision">
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>model://meshes/blueview_p900/COLLISION-p900.dae</uri>
+          </mesh>
+        </geometry>
+      </collision>
+    </link>
+    <static>1</static>
+    <allow_auto_disable>1</allow_auto_disable>
+  </model>
+</sdf>
diff --git a/models/dave_sensor_models/package.xml b/models/dave_sensor_models/package.xml
index 3e62756c..a6148127 100644
--- a/models/dave_sensor_models/package.xml
+++ b/models/dave_sensor_models/package.xml
@@ -9,6 +9,6 @@
   <export>
     <build_type>ament_cmake</build_type>
     <!-- This doesn't work yet https://github.com/gazebosim/ros_gz/pull/492 -->
-    <!-- <gazebo_ros gazebo_model_path="${prefix}/meshes"/> -->
+    <gazebo_ros gazebo_model_path="${prefix}/meshes"/>
   </export>
 </package>
\ No newline at end of file
diff --git a/models/dave_worlds/worlds/lidar_3d.world b/models/dave_worlds/worlds/lidar_3d.world
new file mode 100644
index 00000000..302e9a41
--- /dev/null
+++ b/models/dave_worlds/worlds/lidar_3d.world
@@ -0,0 +1,210 @@
+<?xml version="1.0"?>
+
+<sdf version="1.6">
+  <world name="default">
+    <physics name="1ms" type="ignored">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+    <plugin
+      filename="gz-sim-physics-system"
+      name="gz::sim::systems::Physics">
+    </plugin>
+    <plugin
+      filename="gz-sim-sensors-system"
+      name="gz::sim::systems::Sensors">
+      <render_engine>ogre2</render_engine>
+    </plugin>
+    <plugin
+      filename="gz-sim-user-commands-system"
+      name="gz::sim::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="gz-sim-scene-broadcaster-system"
+      name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+    
+
+    <gui fullscreen="0">
+      <!-- 3D scene -->
+      <plugin filename="MinimalScene" name="3D View">
+        <gz-gui>
+          <title>3D View</title>
+          <property type="bool" key="showTitleBar">false</property>
+          <property type="string" key="state">docked</property>
+        </gz-gui>
+
+        <engine>ogre2</engine>
+        <scene>scene</scene>
+        <ambient_light>0.4 0.4 0.4</ambient_light>
+        <background_color>0.8 0.8 0.8</background_color>
+        <camera_pose>-6 0 6 0 0.5 0</camera_pose>
+      </plugin>
+
+      <!-- Plugins that add functionality to the scene -->
+      <plugin filename="EntityContextMenuPlugin" name="Entity context menu">
+        <gz-gui>
+          <property key="state" type="string">floating</property>
+          <property key="width" type="double">5</property>
+          <property key="height" type="double">5</property>
+          <property key="showTitleBar" type="bool">false</property>
+        </gz-gui>
+      </plugin>
+      <plugin filename="GzSceneManager" name="Scene Manager">
+        <gz-gui>
+          <property key="resizable" type="bool">false</property>
+          <property key="width" type="double">5</property>
+          <property key="height" type="double">5</property>
+          <property key="state" type="string">floating</property>
+          <property key="showTitleBar" type="bool">false</property>
+        </gz-gui>
+      </plugin>
+      <plugin filename="InteractiveViewControl" name="Interactive view control">
+        <gz-gui>
+          <property key="resizable" type="bool">false</property>
+          <property key="width" type="double">5</property>
+          <property key="height" type="double">5</property>
+          <property key="state" type="string">floating</property>
+          <property key="showTitleBar" type="bool">false</property>
+        </gz-gui>
+      </plugin>
+      <plugin filename="CameraTracking" name="Camera Tracking">
+        <gz-gui>
+          <property key="resizable" type="bool">false</property>
+          <property key="width" type="double">5</property>
+          <property key="height" type="double">5</property>
+          <property key="state" type="string">floating</property>
+          <property key="showTitleBar" type="bool">false</property>
+        </gz-gui>
+      </plugin>
+
+      <!-- World control -->
+      <plugin filename="WorldControl" name="World control">
+        <gz-gui>
+          <title>World control</title>
+          <property type="bool" key="showTitleBar">false</property>
+          <property type="bool" key="resizable">false</property>
+          <property type="double" key="height">72</property>
+          <property type="double" key="z">1</property>
+
+          <property type="string" key="state">floating</property>
+          <anchors target="3D View">
+            <line own="left" target="left"/>
+            <line own="bottom" target="bottom"/>
+          </anchors>
+        </gz-gui>
+
+        <play_pause>true</play_pause>
+        <step>true</step>
+        <start_paused>false</start_paused>
+        <use_event>true</use_event>
+      </plugin>
+
+      <!-- World statistics -->
+      <plugin filename="WorldStats" name="World stats">
+        <gz-gui>
+          <title>World stats</title>
+          <property type="bool" key="showTitleBar">false</property>
+          <property type="bool" key="resizable">false</property>
+          <property type="double" key="height">110</property>
+          <property type="double" key="width">290</property>
+          <property type="double" key="z">1</property>
+
+          <property type="string" key="state">floating</property>
+          <anchors target="3D View">
+            <line own="right" target="right"/>
+            <line own="bottom" target="bottom"/>
+          </anchors>
+        </gz-gui>
+
+        <sim_time>true</sim_time>
+        <real_time>true</real_time>
+        <real_time_factor>true</real_time_factor>
+        <iterations>true</iterations>
+      </plugin>
+
+      <!-- Inspector -->
+      <plugin filename="ComponentInspector" name="Component inspector">
+        <gz-gui>
+          <property type="string" key="state">docked</property>
+        </gz-gui>
+      </plugin>
+
+      <!-- Entity tree -->
+      <plugin filename="EntityTree" name="Entity tree">
+        <gz-gui>
+          <property type="string" key="state">docked</property>
+        </gz-gui>
+      </plugin>
+
+      <!-- Camera Viewer -->
+      <plugin filename="ImageDisplay" name="Image Display">
+        <gz-gui>
+          <title>RGB camera</title>
+          <property key="state" type="string">floating</property>
+          <property type="double" key="width">350</property>
+          <property type="double" key="height">315</property>
+        </gz-gui>
+        <topic>/sensor/camera</topic>
+        <topic_picker>false</topic_picker>
+      </plugin>
+      <plugin filename="ImageDisplay" name="Image Display 2">
+        <gz-gui>
+          <title>Depth camera</title>
+          <property key="state" type="string">floating</property>
+          <property type="double" key="width">350</property>
+          <property type="double" key="height">315</property>
+          <property type="double" key="x">500</property>
+        </gz-gui>
+        <topic>/sensor/depth_camera</topic>
+        <topic_picker>false</topic_picker>
+      </plugin>
+
+      <!-- <plugin filename="PointCloud" name="Point Cloud">
+        <gz-gui>
+          <title>3D LiDAR Points</title>
+          <property type="string" key="state">docked</property>
+        </gz-gui>
+        <point_cloud_topic>/lidar_3d/lidar/points</point_cloud_topic>
+      </plugin> -->
+
+    </gui>
+
+    <include>
+      <uri>
+        https://fuel.gazebosim.org/1.0/OpenRobotics/models/Ground Plane
+      </uri>
+      <pose>0 0 0 0 0 0</pose>
+    </include>
+
+    <include>
+      <uri>
+        https://fuel.gazebosim.org/1.0/OpenRobotics/models/Sun
+      </uri>
+    </include>
+
+    <include>
+      <pose>6 -2 0 0 0 1.57</pose>
+      <uri>
+      https://fuel.gazebosim.org/1.0/hmoyen/models/basement tank
+      </uri>
+    </include>
+    <!-- cylinder target 1 -->
+    <include>
+      <uri>https://fuel.gazebosim.org/1.0/hmoyen/models/cylinder target</uri>
+      <name>cylinder_target1</name>
+      <pose>2.1 1 2.0 0 0 0</pose>
+      <static>true</static>
+    </include>
+
+    <!-- cylinder target 2 -->
+    <include>
+      <uri>https://fuel.gazebosim.org/1.0/hmoyen/models/cylinder target</uri>
+      <name>cylinder_target2</name>
+      <pose>3.1 1 1.5 0 1.5709 1.57</pose>
+      <static>true</static>
+    </include>
+
+
+  </world>
+</sdf>