254 lines
8.7 KiB
Java
254 lines
8.7 KiB
Java
package com.dongni.collisionavoidance.dataProcessing.service;
|
|
|
|
import com.dongni.collisionavoidance.common.model.GeoPosition;
|
|
import com.dongni.collisionavoidance.common.model.MovementState;
|
|
import com.dongni.collisionavoidance.common.model.MovingObject;
|
|
import com.dongni.collisionavoidance.common.model.Velocity;
|
|
import org.springframework.stereotype.Service;
|
|
|
|
import java.util.ArrayDeque;
|
|
import java.util.Deque;
|
|
import java.util.List;
|
|
import java.util.concurrent.locks.ReentrantLock;
|
|
|
|
@Service
|
|
public class SpeedCalculationService {
|
|
// 滑动窗口大小（秒）
|
|
private static final int TAKEOFF_WINDOW_SIZE = 10;
|
|
private static final int LANDING_WINDOW_SIZE = 5;
|
|
|
|
// 加速度范围（米/秒²）
|
|
private static final double MIN_TAKEOFF_ACCELERATION = 0.5;
|
|
private static final double MAX_TAKEOFF_ACCELERATION = 2.5;
|
|
private static final double MIN_LANDING_ACCELERATION = -2.0;
|
|
private static final double MAX_LANDING_ACCELERATION = 0.0;
|
|
|
|
// 异常值检测阈值
|
|
private static final double MAX_POSITION_JUMP = 100.0; // 米
|
|
private static final double MAX_TIME_JUMP = 5000.0; // 毫秒
|
|
|
|
/**
|
|
* 数据预处理
|
|
*/
|
|
private final ReentrantLock lock = new ReentrantLock();
|
|
|
|
public void preprocessData(List<MovingObject> dataList) {
|
|
if (dataList == null || dataList.isEmpty()) {
|
|
return;
|
|
}
|
|
|
|
lock.lock();
|
|
try {
|
|
for (MovingObject obj : dataList) {
|
|
|
|
// 如果历史状态队列为空，使用当前状态初始化
|
|
if (obj.getStateHistory().isEmpty()) {
|
|
MovementState state = new MovementState();
|
|
state.setPosition(obj.getCurrentPosition());
|
|
state.setVelocity(obj.getVelocity());
|
|
state.setTimestamp(obj.getTimestamp());
|
|
|
|
// 默认设置数据质量为GOOD
|
|
state.setDataQuality(MovementState.DataQuality.GOOD);
|
|
obj.getStateHistory().add(state);
|
|
continue;
|
|
}
|
|
|
|
// 获取最近的历史状态
|
|
MovementState lastState = obj.getStateHistory().getFirst();
|
|
|
|
// 异常值检测
|
|
double distance = calculateDistance(obj.getVelocity(), lastState.getVelocity());
|
|
long timeDiff = obj.getTimestamp() - lastState.getTimestamp();
|
|
|
|
// 检查位置跳变
|
|
if (distance > MAX_POSITION_JUMP) {
|
|
obj.getStateHistory().getFirst().setDataQuality(MovementState.DataQuality.SUSPICIOUS);
|
|
// state.setDataQuality(MovementState.DataQuality.SUSPICIOUS);
|
|
}
|
|
|
|
// 检查时间异常
|
|
if (timeDiff <= 0 || timeDiff > MAX_TIME_JUMP) {
|
|
obj.getStateHistory().getFirst().setDataQuality(MovementState.DataQuality.SUSPICIOUS);
|
|
// state.setDataQuality(MovementState.DataQuality.SUSPICIOUS);
|
|
}
|
|
|
|
// 检查速度异常
|
|
double speed = distance / (timeDiff / 1000.0);
|
|
if (speed > obj.getMaxSpeed()) {
|
|
obj.getStateHistory().getFirst().setDataQuality(MovementState.DataQuality.SUSPICIOUS);
|
|
// state.setDataQuality(MovementState.DataQuality.SUSPICIOUS);
|
|
}
|
|
|
|
// 只对质量良好的数据进行卡尔曼滤波
|
|
// if (object.getStateHistory().getLast().getDataQuality() == MovementState.DataQuality.GOOD) {
|
|
// applyKalmanFilter(object.getStateHistory().getLast(), object.getStateHistory());
|
|
// }
|
|
|
|
// object.getStateHistory().add(state);
|
|
}
|
|
}finally {
|
|
lock.unlock();
|
|
}
|
|
|
|
}
|
|
|
|
/**
|
|
* 异常值检测
|
|
*/
|
|
private boolean isDataAnomaly(MovingObject object, GeoPosition newPosition, long timestamp) {
|
|
if (object.getStateHistory().isEmpty()) {
|
|
return false;
|
|
}
|
|
|
|
MovementState lastState = object.getStateHistory().getLast();
|
|
|
|
// 检查位置跳变
|
|
double distance = calculateDistance(lastState.getVelocity(), object.getVelocity());
|
|
if (distance > MAX_POSITION_JUMP) {
|
|
return true;
|
|
}
|
|
|
|
// 检查时间异常
|
|
long timeDiff = timestamp - lastState.getTimestamp();
|
|
if (timeDiff <= 0 || timeDiff > MAX_TIME_JUMP) {
|
|
return true;
|
|
}
|
|
|
|
// 检查速度异常
|
|
double speed = distance / (timeDiff / 1000.0);
|
|
return speed > object.getMaxSpeed();
|
|
}
|
|
|
|
/**
|
|
* 卡尔曼滤波
|
|
*/
|
|
private void applyKalmanFilter(MovementState state, Deque<MovementState> history) {
|
|
// TODO: 实现卡尔曼滤波算法
|
|
// 状态向量：[x, y, vx, vy]
|
|
// 观测向量：[x, y]
|
|
}
|
|
|
|
/**
|
|
* 计算速度
|
|
*/
|
|
public void calculateSpeed(MovingObject object, MovementState newState) {
|
|
// Deque<MovementState> history = object.getStateHistory();
|
|
// if (history.isEmpty()) {
|
|
// return;
|
|
// }
|
|
//
|
|
// // 确定窗口大小
|
|
// int windowSize = determineWindowSize(object);
|
|
//
|
|
// // 计算速度
|
|
// Velocity velocity = calculateVelocityComponents(object, object.getStateHistory().getLast());
|
|
//
|
|
// // 应用加速度约束
|
|
// applyAccelerationConstraints(velocity, object);
|
|
//
|
|
// newState.setVelocity(velocity);
|
|
}
|
|
|
|
/**
|
|
* 确定滑动窗口大小
|
|
*/
|
|
private int determineWindowSize(MovingObject object) {
|
|
if (isInTakeoffState(object)) {
|
|
return TAKEOFF_WINDOW_SIZE;
|
|
} else if (isInLandingState(object)) {
|
|
return LANDING_WINDOW_SIZE;
|
|
}
|
|
return TAKEOFF_WINDOW_SIZE; // 默认使用起飞窗口大小
|
|
}
|
|
|
|
/**
|
|
* 判断是否在起飞状态
|
|
*/
|
|
private boolean isInTakeoffState(MovingObject object) {
|
|
// TODO: 实现起飞状态判断逻辑
|
|
return false;
|
|
}
|
|
|
|
/**
|
|
* 判断是否在降落状态
|
|
*/
|
|
private boolean isInLandingState(MovingObject object) {
|
|
// TODO: 实现降落状态判断逻辑
|
|
return false;
|
|
}
|
|
|
|
/**
|
|
* 从滑动窗口计算速度
|
|
*/
|
|
private void calculateVelocityComponents(MovingObject object, MovementState lastState) {
|
|
|
|
Velocity velocity = object.getVelocity();
|
|
|
|
long currentTime = object.getTimestamp();
|
|
long lastTime = lastState.getTimestamp();
|
|
long deltaTime = currentTime - lastTime;
|
|
|
|
if (deltaTime <= 0) {
|
|
object.getStateHistory().getLast().setDataQuality(MovementState.DataQuality.SUSPICIOUS);
|
|
return;
|
|
}
|
|
|
|
double deltaX = object.getVelocity().getX() - lastState.getVelocity().getX();
|
|
double deltaY = object.getVelocity().getY() - lastState.getVelocity().getY();
|
|
double deltaZ = object.getVelocity().getZ() - lastState.getVelocity().getZ();
|
|
|
|
object.getVelocity().setVx(deltaX / (deltaTime / 1000.0));
|
|
object.getVelocity().setVy(deltaY / (deltaTime / 1000.0));
|
|
object.getVelocity().setVz(deltaZ / (deltaTime / 1000.0));
|
|
|
|
// 缓存瞬时加速度
|
|
double lastSpeed = Math.sqrt(
|
|
Math.pow(lastState.getVelocity().getVx(), 2) +
|
|
Math.pow(lastState.getVelocity().getVy(), 2) +
|
|
Math.pow(lastState.getVelocity().getVz(), 2)
|
|
);
|
|
double currentSpeed = Math.sqrt(Math.pow(velocity.getVx(), 2) + Math.pow(velocity.getVy(), 2));
|
|
object.getVelocity().setCachedAcceleration((currentSpeed - lastSpeed) / (deltaTime / 1000.0));
|
|
}
|
|
|
|
/**
|
|
* 应用加速度约束
|
|
*/
|
|
private void applyAccelerationConstraints(Velocity velocity, MovingObject object) {
|
|
double acceleration = velocity.getCachedAcceleration();
|
|
|
|
if (isInTakeoffState(object)) {
|
|
if (acceleration < MIN_TAKEOFF_ACCELERATION) {
|
|
velocity.setConfidence(0.5);
|
|
} else if (acceleration > MAX_TAKEOFF_ACCELERATION) {
|
|
velocity.setConfidence(0.5);
|
|
} else {
|
|
velocity.setConfidence(1.0);
|
|
}
|
|
} else if (isInLandingState(object)) {
|
|
if (acceleration < MIN_LANDING_ACCELERATION) {
|
|
velocity.setConfidence(0.5);
|
|
} else if (acceleration > MAX_LANDING_ACCELERATION) {
|
|
velocity.setConfidence(0.5);
|
|
} else {
|
|
velocity.setConfidence(1.0);
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* 计算两个向量间的距离
|
|
* @param currentVelocity 当前向量
|
|
* @param historicalVelocity 历史向量
|
|
* @return 距离
|
|
*/
|
|
private double calculateDistance(Velocity currentVelocity, Velocity historicalVelocity) {
|
|
|
|
double deltaX = currentVelocity.getX() - historicalVelocity.getX();
|
|
double deltaY = currentVelocity.getY() - historicalVelocity.getY();
|
|
double deltaZ = currentVelocity.getZ() - historicalVelocity.getZ();
|
|
return Math.sqrt(deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ);
|
|
}
|
|
}
|