263 lines
9.8 KiB
C++
263 lines
9.8 KiB
C++
#include <opencv2/opencv.hpp>
|
|
#include <Eigen/Dense>
|
|
#include <vector>
|
|
#include <map>
|
|
#include <spdlog/spdlog.h>
|
|
#include <chrono>
|
|
#include <thread>
|
|
#include <mutex>
|
|
|
|
#include "aoa_estimator.cpp"
|
|
|
|
class RFSLAM {
|
|
private:
|
|
AoAEstimator aoaEstimator;
|
|
std::map<std::string, Eigen::Vector3d> landmarks;
|
|
std::vector<Eigen::Vector3d> trajectory;
|
|
std::mutex slamMutex;
|
|
|
|
// SLAM parameters
|
|
double processNoise;
|
|
double measurementNoise;
|
|
Eigen::Matrix3d processCovariance;
|
|
Eigen::Matrix2d measurementCovariance;
|
|
|
|
// Current state
|
|
Eigen::Vector3d currentPose; // x, y, theta
|
|
Eigen::Matrix3d currentCovariance;
|
|
|
|
public:
|
|
RFSLAM(int numAntennas, int numSubcarriers, double frequency)
|
|
: aoaEstimator(numAntennas, numSubcarriers, frequency) {
|
|
|
|
// Initialize SLAM parameters
|
|
processNoise = 0.1;
|
|
measurementNoise = 0.05;
|
|
|
|
processCovariance = Eigen::Matrix3d::Identity() * processNoise;
|
|
measurementCovariance = Eigen::Matrix2d::Identity() * measurementNoise;
|
|
|
|
// Initialize pose
|
|
currentPose = Eigen::Vector3d::Zero();
|
|
currentCovariance = Eigen::Matrix3d::Identity() * 0.1;
|
|
|
|
spdlog::info("RF SLAM initialized");
|
|
}
|
|
|
|
struct SLAMResult {
|
|
Eigen::Vector3d pose;
|
|
Eigen::Matrix3d covariance;
|
|
std::vector<Eigen::Vector3d> landmarks;
|
|
double confidence;
|
|
};
|
|
|
|
SLAMResult update(const std::vector<std::vector<std::vector<std::complex<double>>>>& csiData,
|
|
const std::string& sourceMac) {
|
|
std::lock_guard<std::mutex> lock(slamMutex);
|
|
|
|
SLAMResult result;
|
|
result.pose = currentPose;
|
|
result.covariance = currentCovariance;
|
|
|
|
// Estimate AoA from CSI data
|
|
auto aoaResult = aoaEstimator.estimateAoA(csiData);
|
|
|
|
if (aoaResult.confidence > 0.1) { // Threshold for reliable measurement
|
|
// Predict step (motion model)
|
|
Eigen::Vector3d predictedPose = currentPose;
|
|
Eigen::Matrix3d predictedCovariance = currentCovariance + processCovariance;
|
|
|
|
// Update step (measurement model)
|
|
Eigen::Vector2d measurement(aoaResult.azimuth * M_PI / 180.0, aoaResult.elevation * M_PI / 180.0);
|
|
|
|
// Check if this is a known landmark
|
|
auto landmarkIt = landmarks.find(sourceMac);
|
|
if (landmarkIt != landmarks.end()) {
|
|
// Known landmark - update pose
|
|
result = updateWithKnownLandmark(predictedPose, predictedCovariance,
|
|
measurement, landmarkIt->second);
|
|
} else {
|
|
// New landmark - initialize and update
|
|
Eigen::Vector3d newLandmark = estimateLandmarkPosition(predictedPose, measurement);
|
|
landmarks[sourceMac] = newLandmark;
|
|
|
|
result = updateWithNewLandmark(predictedPose, predictedCovariance, measurement, newLandmark);
|
|
}
|
|
|
|
// Update current state
|
|
currentPose = result.pose;
|
|
currentCovariance = result.covariance;
|
|
|
|
// Add to trajectory
|
|
trajectory.push_back(currentPose);
|
|
|
|
spdlog::info("RF SLAM update - Pose: ({:.2f}, {:.2f}, {:.2f}), Confidence: {:.3f}",
|
|
currentPose.x(), currentPose.y(), currentPose.z(), aoaResult.confidence);
|
|
}
|
|
|
|
// Copy landmarks to result
|
|
for (const auto& landmark : landmarks) {
|
|
result.landmarks.push_back(landmark.second);
|
|
}
|
|
|
|
result.confidence = aoaResult.confidence;
|
|
return result;
|
|
}
|
|
|
|
void saveTrajectory(const std::string& filename) {
|
|
std::ofstream file(filename);
|
|
if (file.is_open()) {
|
|
for (const auto& pose : trajectory) {
|
|
file << pose.x() << " " << pose.y() << " " << pose.z() << std::endl;
|
|
}
|
|
file.close();
|
|
spdlog::info("Trajectory saved to: {}", filename);
|
|
}
|
|
}
|
|
|
|
void saveLandmarks(const std::string& filename) {
|
|
std::ofstream file(filename);
|
|
if (file.is_open()) {
|
|
for (const auto& landmark : landmarks) {
|
|
file << landmark.first << " "
|
|
<< landmark.second.x() << " "
|
|
<< landmark.second.y() << " "
|
|
<< landmark.second.z() << std::endl;
|
|
}
|
|
file.close();
|
|
spdlog::info("Landmarks saved to: {}", filename);
|
|
}
|
|
}
|
|
|
|
private:
|
|
SLAMResult updateWithKnownLandmark(const Eigen::Vector3d& predictedPose,
|
|
const Eigen::Matrix3d& predictedCovariance,
|
|
const Eigen::Vector2d& measurement,
|
|
const Eigen::Vector3d& landmark) {
|
|
SLAMResult result;
|
|
|
|
// Measurement Jacobian
|
|
Eigen::Matrix<double, 2, 3> H = Eigen::Matrix<double, 2, 3>::Zero();
|
|
|
|
double dx = landmark.x() - predictedPose.x();
|
|
double dy = landmark.y() - predictedPose.y();
|
|
double range = sqrt(dx*dx + dy*dy);
|
|
|
|
if (range > 0.001) { // Avoid division by zero
|
|
H(0, 0) = -dy / (range * range); // d(azimuth)/dx
|
|
H(0, 1) = dx / (range * range); // d(azimuth)/dy
|
|
H(1, 0) = dx * (landmark.z() - predictedPose.z()) / (range * range * range); // d(elevation)/dx
|
|
H(1, 1) = dy * (landmark.z() - predictedPose.z()) / (range * range * range); // d(elevation)/dy
|
|
H(1, 2) = -range / (range * range * range); // d(elevation)/dz
|
|
}
|
|
|
|
// Kalman gain
|
|
Eigen::Matrix<double, 3, 2> K = predictedCovariance * H.transpose() *
|
|
(H * predictedCovariance * H.transpose() + measurementCovariance).inverse();
|
|
|
|
// Update
|
|
Eigen::Vector2d predictedMeasurement = measurementModel(predictedPose, landmark);
|
|
Eigen::Vector2d innovation = measurement - predictedMeasurement;
|
|
|
|
result.pose = predictedPose + K * innovation;
|
|
result.covariance = (Eigen::Matrix3d::Identity() - K * H) * predictedCovariance;
|
|
|
|
return result;
|
|
}
|
|
|
|
SLAMResult updateWithNewLandmark(const Eigen::Vector3d& predictedPose,
|
|
const Eigen::Matrix3d& predictedCovariance,
|
|
const Eigen::Vector2d& measurement,
|
|
const Eigen::Vector3d& newLandmark) {
|
|
SLAMResult result;
|
|
|
|
// For new landmarks, we use a simplified update
|
|
// In practice, you'd extend the state vector to include landmark positions
|
|
|
|
result.pose = predictedPose;
|
|
result.covariance = predictedCovariance;
|
|
|
|
return result;
|
|
}
|
|
|
|
Eigen::Vector3d estimateLandmarkPosition(const Eigen::Vector3d& pose, const Eigen::Vector2d& measurement) {
|
|
// Simple landmark position estimation
|
|
// In practice, you'd use triangulation from multiple measurements
|
|
|
|
double azimuth = measurement.x();
|
|
double elevation = measurement.y();
|
|
|
|
// Assume landmark is at a fixed distance (e.g., 10 meters)
|
|
double distance = 10.0;
|
|
|
|
Eigen::Vector3d landmark;
|
|
landmark.x() = pose.x() + distance * cos(azimuth) * cos(elevation);
|
|
landmark.y() = pose.y() + distance * sin(azimuth) * cos(elevation);
|
|
landmark.z() = pose.z() + distance * sin(elevation);
|
|
|
|
return landmark;
|
|
}
|
|
|
|
Eigen::Vector2d measurementModel(const Eigen::Vector3d& pose, const Eigen::Vector3d& landmark) {
|
|
double dx = landmark.x() - pose.x();
|
|
double dy = landmark.y() - pose.y();
|
|
double dz = landmark.z() - pose.z();
|
|
|
|
double azimuth = atan2(dy, dx);
|
|
double elevation = atan2(dz, sqrt(dx*dx + dy*dy));
|
|
|
|
return Eigen::Vector2d(azimuth, elevation);
|
|
}
|
|
};
|
|
|
|
int main(int argc, char** argv) {
|
|
if (argc < 2) {
|
|
std::cout << "Usage: " << argv[0] << " <csi_data_file> [output_dir]" << std::endl;
|
|
return 1;
|
|
}
|
|
|
|
std::string csiFile = argv[1];
|
|
std::string outputDir = (argc > 2) ? argv[2] : "./rf_slam_output";
|
|
|
|
// Create output directory
|
|
std::filesystem::create_directories(outputDir);
|
|
|
|
// Initialize RF SLAM
|
|
RFSLAM slam(3, 30, 5.2e9); // 3 antennas, 30 subcarriers, 5.2GHz
|
|
|
|
spdlog::info("RF SLAM started. Processing CSI data from: {}", csiFile);
|
|
|
|
// In practice, you would read CSI data from a file or real-time stream
|
|
// For demonstration, we'll create synthetic data
|
|
std::vector<std::vector<std::vector<std::complex<double>>>> csiData;
|
|
csiData.resize(3); // 3 antennas
|
|
|
|
for (int ant = 0; ant < 3; ++ant) {
|
|
csiData[ant].resize(30); // 30 subcarriers
|
|
for (int sc = 0; sc < 30; ++sc) {
|
|
// Synthetic CSI with some phase variation
|
|
double phase = 2 * M_PI * sc * 0.1 + ant * M_PI / 4;
|
|
csiData[ant][sc] = std::complex<double>(cos(phase), sin(phase));
|
|
}
|
|
}
|
|
|
|
// Process multiple measurements
|
|
for (int i = 0; i < 10; ++i) {
|
|
auto result = slam.update(csiData, "00:11:22:33:44:55");
|
|
|
|
spdlog::info("Step {}: Pose=({:.2f}, {:.2f}, {:.2f}), Landmarks={}, Confidence={:.3f}",
|
|
i, result.pose.x(), result.pose.y(), result.pose.z(),
|
|
result.landmarks.size(), result.confidence);
|
|
|
|
// Simulate some movement
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
}
|
|
|
|
// Save results
|
|
slam.saveTrajectory(outputDir + "/trajectory.txt");
|
|
slam.saveLandmarks(outputDir + "/landmarks.txt");
|
|
|
|
spdlog::info("RF SLAM completed. Results saved to: {}", outputDir);
|
|
|
|
return 0;
|
|
}
|