bc 1.53
This commit is contained in:
Joshua
0
AUTOSAMPLERIND/v1/1000)
Normal file
0
AUTOSAMPLERIND/v1/1000)
Normal file
0
AUTOSAMPLERIND/v1/a+pt.w
Normal file
0
AUTOSAMPLERIND/v1/a+pt.w
Normal file
142
AUTOSAMPLERIND/v1/index.html
Normal file
142
AUTOSAMPLERIND/v1/index.html
Normal file
@@ -0,0 +1,142 @@
|
||||
<!DOCTYPE html>
|
||||
<html>
|
||||
<head>
|
||||
<title>Particle Filter Tracker – responsive update</title>
|
||||
<meta charset="utf-8" />
|
||||
<style>
|
||||
html, body { margin: 0; height: 100%; }
|
||||
#map { width: 100%; height: 100%; }
|
||||
</style>
|
||||
<link rel="stylesheet" href="https://unpkg.com/leaflet@1.9.4/dist/leaflet.css" />
|
||||
<script src="https://unpkg.com/leaflet@1.9.4/dist/leaflet.js"></script>
|
||||
<script src="https://cdn.jsdelivr.net/npm/leaflet-ellipse@1.0.0/leaflet-ellipse.min.js"></script>
|
||||
</head>
|
||||
<body>
|
||||
<div id="map"></div>
|
||||
<script>
|
||||
/*************
|
||||
* SETTINGS
|
||||
*************/
|
||||
const MAX_DRAW = 400; // max particles drawn per cloud each tick (for speed)
|
||||
const JITTER = 0.0005; // resample noise
|
||||
|
||||
/*************
|
||||
* MAP INIT
|
||||
*************/
|
||||
const map = L.map('map').setView([-41.5917,145.9344],9);
|
||||
L.tileLayer('https://{s}.tile.openstreetmap.org/{z}/{x}/{y}.png').addTo(map);
|
||||
|
||||
/*************
|
||||
* GLOBAL STATE
|
||||
*************/
|
||||
const detectionCounts = {}, bearingCount = {}, orbitTriggered = {};
|
||||
const targets = {};
|
||||
let droneMarker=null, droneTrail=[], droneTrailLine=null, dronePos=null;
|
||||
|
||||
/*************
|
||||
* UTILITIES
|
||||
*************/
|
||||
const toRad=d=>d*Math.PI/180;
|
||||
function angularDifference(a,b){let d=Math.abs(a-b)%360; return d>180?360-d:d;}
|
||||
function calculateBearing(lat1,lon1,lat2,lon2){const y=Math.sin(toRad(lon2-lon1))*Math.cos(toRad(lat2)); const x=Math.cos(toRad(lat1))*Math.sin(toRad(lat2))-Math.sin(toRad(lat1))*Math.cos(toRad(lat2))*Math.cos(toRad(lon2-lon1)); return (Math.atan2(y,x)*180/Math.PI+360)%360;}
|
||||
function haversine(lat1,lon1,lat2,lon2){const R=6371; const dLat=toRad(lat2-lat1); const dLon=toRad(lon2-lon1); const a=Math.sin(dLat/2)**2+Math.cos(toRad(lat1))*Math.cos(toRad(lat2))*Math.sin(dLon/2)**2; return R*2*Math.atan2(Math.sqrt(a),Math.sqrt(1-a));}
|
||||
|
||||
/*************
|
||||
* COLOURS & GROUND‑TRUTH
|
||||
*************/
|
||||
const FREQ_COLORS={ '170.0':'red','170.1':'blue','170.2':'green','170.3':'orange','170.4':'purple','170.5':'brown','170.6':'cyan','170.7':'magenta','170.8':'lime','170.9':'teal','171.0':'pink','171.1':'navy','171.2':'gold','171.3':'salmon','171.4':'olive','171.5':'maroon','171.6':'coral','171.7':'indigo','171.8':'violet','171.9':'gray'};
|
||||
const actualPositions={
|
||||
'170.0':[-41.1568,146.4257],'170.1':[-41.3400,146.2625],'170.2':[-41.8190,146.4390],'170.3':[-42.0139,145.6077],'170.4':[-41.6302,146.3656],'170.5':[-41.6880,146.1030],'170.6':[-41.3899,146.3310],'170.7':[-41.4410,145.5380],'170.8':[-42.0270,146.3340],'170.9':[-41.4960,146.4300],'171.0':[-41.7510,146.2030],'171.1':[-41.8950,146.4310],'171.2':[-41.2390,145.7600],'171.3':[-41.3730,145.5870],'171.4':[-41.2900,145.8420],'171.5':[-41.8770,145.7510],'171.6':[-42.0130,145.9500],'171.7':[-41.5917,145.9344],'171.8':[-41.8000,145.5000],'171.9':[-41.5000,146.0000]
|
||||
};
|
||||
for(const [f,pos] of Object.entries(actualPositions)){
|
||||
L.marker(pos,{icon:L.divIcon({className:'actual',html:`<div style=\"background:${FREQ_COLORS[f]};width:10px;height:10px;border-radius:50%;border:2px solid #fff\"></div>`})}).addTo(map);
|
||||
}
|
||||
|
||||
/*************
|
||||
* PARTICLE FILTER CLASS
|
||||
*************/
|
||||
class ParticleFilter{
|
||||
constructor(N,bounds){this.N=N;this.bounds=bounds;this.spawn();}
|
||||
spawn(){this.p=[];for(let i=0;i<this.N;i++){this.p.push({lat:this.bounds.latMin+Math.random()*(this.bounds.latMax-this.bounds.latMin),lon:this.bounds.lonMin+Math.random()*(this.bounds.lonMax-this.bounds.lonMin),w:1});}}
|
||||
reseedAround(lat,lon,rKm=0.1,frac=0.3){const n=Math.floor(this.N*frac);const rDeg=rKm/111;for(let i=0;i<n;i++){const a=Math.random()*2*Math.PI;this.p[i]={lat:lat+rDeg*Math.sin(a),lon:lon+rDeg*Math.cos(a),w:1};}}
|
||||
update(dLat,dLon,bear,str){for(const pt of this.p){const exp=calculateBearing(dLat,dLon,pt.lat,pt.lon);const err=angularDifference(exp,bear);pt.w = Math.exp(-((err/10) ** 2)) * str;}
|
||||
this.normalize();this.resample();}
|
||||
normalize(){let s=this.p.reduce((a,pt)=>a+pt.w,0); if(!s){this.spawn();return;} for(const pt of this.p)pt.w/=s; }
|
||||
resample(){const cum=[];let s=0;for(const pt of this.p){s+=pt.w;cum.push(s);}const newP=[];for(let i=0;i<this.N;i++){const r=Math.random();const idx=cum.findIndex(c=>c>r);const cpt=this.p[idx]||this.p[0];newP.push({lat:cpt.lat+(Math.random()-0.5)*JITTER,lon:cpt.lon+(Math.random()-0.5)*JITTER,w:1});}this.p=newP;}
|
||||
estimate(){let lat=0,lon=0,sum=0;for(const pt of this.p){lat+=pt.lat*pt.w;lon+=pt.lon*pt.w;sum+=pt.w;}return{lat:lat/sum,lon:lon/sum,conf:sum/this.N};}
|
||||
}
|
||||
|
||||
/*************
|
||||
* DRAW HELPERS – reuse circleMarkers
|
||||
*************/
|
||||
function syncParticleMarkers(tgt){
|
||||
const want=Math.min(MAX_DRAW,tgt.pf.p.length);
|
||||
// extend list
|
||||
while(tgt.particleMarkers.length<want){tgt.particleMarkers.push(L.circleMarker([0,0],{radius:2,color:tgt.color,opacity:0.5,fillOpacity:0.3,weight:0.5}).addTo(map));}
|
||||
// shrink list
|
||||
while(tgt.particleMarkers.length>want){const m=tgt.particleMarkers.pop();map.removeLayer(m);}
|
||||
for(let i=0;i<want;i++) tgt.particleMarkers[i].setLatLng([tgt.pf.p[i].lat,tgt.pf.p[i].lon]);
|
||||
}
|
||||
|
||||
/*************
|
||||
* WEBSOCKET HANDLER
|
||||
*************/
|
||||
const ws=new WebSocket('ws://localhost:8765');
|
||||
ws.onmessage=e=>{const m=JSON.parse(e.data);
|
||||
/* ---------- DRONE POSITION ---------- */
|
||||
if(m.type==='drone'){
|
||||
dronePos=[m.lat,m.lon];
|
||||
if(!droneMarker) droneMarker=L.circleMarker(dronePos,{radius:6,color:'#000'}).addTo(map); else droneMarker.setLatLng(dronePos);
|
||||
droneTrail.push(dronePos); if(droneTrail.length>1000) droneTrail.shift();
|
||||
if(!droneTrailLine) droneTrailLine=L.polyline(droneTrail,{color:'#000',weight:1}).addTo(map); else droneTrailLine.setLatLngs(droneTrail);
|
||||
return;
|
||||
}
|
||||
|
||||
/* ---------- BEARING MESSAGE ---------- */
|
||||
if(m.type==='bearing'){
|
||||
const freq=m.freq.toFixed(1);
|
||||
const strength=m.weight??1; if(strength<0.05) return;
|
||||
|
||||
bearingCount[freq]=(bearingCount[freq]||0)+1;
|
||||
detectionCounts[freq]=(detectionCounts[freq]||0)+1;
|
||||
|
||||
if(!targets[freq]){
|
||||
targets[freq]={
|
||||
color:FREQ_COLORS[freq],
|
||||
pf:new ParticleFilter(1000,{latMin:-42.1,latMax:-41.1,lonMin:145.3,lonMax:146.5}),
|
||||
particleMarkers:[], marker:null, line:null
|
||||
};
|
||||
}
|
||||
const tgt=targets[freq];
|
||||
|
||||
/* ---- PARTICLE FILTER UPDATE ---- */
|
||||
tgt.pf.update(m.lat,m.lon,m.bearing,strength);
|
||||
const {lat,lon,conf}=tgt.pf.estimate();
|
||||
|
||||
/* ---- DRAW PARTICLES EFFICIENTLY ---- */
|
||||
syncParticleMarkers(tgt);
|
||||
|
||||
/* ---- ORBIT LOGIC (unchanged) ---- */
|
||||
if(dronePos && detectionCounts[freq]>=20){
|
||||
const dKm=haversine(dronePos[0],dronePos[1],lat,lon);
|
||||
if(!orbitTriggered[freq] || dKm>0.15){
|
||||
const cmd={type:'command',action:'orbit_target',freq:parseFloat(freq),lat,lon,radius:1};
|
||||
console.log('📍 Updating orbit',cmd); ws.send(JSON.stringify(cmd)); orbitTriggered[freq]=true;
|
||||
}else if(dKm<=0.5 && orbitTriggered[freq]===true){
|
||||
console.log(`✅ Final orbit lock‑in for ${freq}`); orbitTriggered[freq]='locked';
|
||||
}
|
||||
}
|
||||
|
||||
/* ---- ESTIMATE MARKER ---- */
|
||||
const rMeters=200*(1-conf)+10; const alpha=Math.min(1,conf*1.5);
|
||||
if(tgt.marker){tgt.marker.setLatLng([lat,lon]); tgt.marker.setRadius(rMeters); tgt.marker.setStyle({fillOpacity:alpha});}
|
||||
else tgt.marker=L.circle([lat,lon],{radius:rMeters,color:tgt.color,fillOpacity:alpha,weight:1}).addTo(map);
|
||||
|
||||
/* ---- DASHED LINE DRONE->EST ---- */
|
||||
if(tgt.line) map.removeLayer(tgt.line);
|
||||
tgt.line=L.polyline([[m.lat,m.lon],[lat,lon]],{color:tgt.color,weight:1,opacity:0.4,dashArray:'5,5'}).addTo(map);
|
||||
}
|
||||
};
|
||||
</script>
|
||||
</body>
|
||||
</html>
|
||||
232
AUTOSAMPLERIND/v1/sim.py
Normal file
232
AUTOSAMPLERIND/v1/sim.py
Normal file
@@ -0,0 +1,232 @@
|
||||
import asyncio
|
||||
import json
|
||||
import math
|
||||
import random
|
||||
import websockets
|
||||
|
||||
FREQ_COLORS = {
|
||||
170.0: "red", 170.1: "blue", 170.2: "green", 170.3: "orange", 170.4: "purple",
|
||||
170.5: "brown", 170.6: "cyan", 170.7: "magenta", 170.8: "lime", 170.9: "teal",
|
||||
171.0: "pink", 171.1: "navy", 171.2: "gold", 171.3: "salmon", 171.4: "olive",
|
||||
171.5: "maroon", 171.6: "coral", 171.7: "indigo", 171.8: "violet", 171.9: "gray"
|
||||
}
|
||||
|
||||
clients = set()
|
||||
commands = asyncio.Queue()
|
||||
|
||||
center_lat, center_lon = -41.5917, 145.9344
|
||||
|
||||
current_lat, current_lon = center_lat, center_lon
|
||||
|
||||
active_orbit_target = None
|
||||
orbit_path = []
|
||||
orbit_index = 0
|
||||
orbit_active = False
|
||||
orbit_queue = []
|
||||
completed_orbits = set()
|
||||
active_freq = None
|
||||
|
||||
# Targets
|
||||
targets = [
|
||||
{"lat": -41.1568, "lon": 146.4257, "freq": 170.0},
|
||||
{"lat": -41.3400, "lon": 146.2625, "freq": 170.1},
|
||||
{"lat": -41.8190, "lon": 146.4390, "freq": 170.2},
|
||||
{"lat": -42.0139, "lon": 145.6077, "freq": 170.3},
|
||||
{"lat": -41.6302, "lon": 146.3656, "freq": 170.4},
|
||||
{"lat": -41.6880, "lon": 146.1030, "freq": 170.5},
|
||||
{"lat": -41.3899, "lon": 146.3310, "freq": 170.6},
|
||||
{"lat": -41.4410, "lon": 145.5380, "freq": 170.7},
|
||||
{"lat": -42.0270, "lon": 146.3340, "freq": 170.8},
|
||||
{"lat": -41.4960, "lon": 146.4300, "freq": 170.9},
|
||||
{"lat": -41.7510, "lon": 146.2030, "freq": 171.0},
|
||||
{"lat": -41.8950, "lon": 146.4310, "freq": 171.1},
|
||||
{"lat": -41.2390, "lon": 145.7600, "freq": 171.2},
|
||||
{"lat": -41.3730, "lon": 145.5870, "freq": 171.3},
|
||||
{"lat": -41.2900, "lon": 145.8420, "freq": 171.4},
|
||||
{"lat": -41.8770, "lon": 145.7510, "freq": 171.5},
|
||||
{"lat": -42.0130, "lon": 145.9500, "freq": 171.6},
|
||||
{"lat": -41.5917, "lon": 145.9344, "freq": 171.7},
|
||||
{"lat": -41.8000, "lon": 145.5000, "freq": 171.8},
|
||||
{"lat": -41.5000, "lon": 146.0000, "freq": 171.9}
|
||||
]
|
||||
|
||||
def haversine(lat1, lon1, lat2, lon2):
|
||||
R = 6371
|
||||
dlat = math.radians(lat2 - lat1)
|
||||
dlon = math.radians(lon2 - lon1)
|
||||
a = math.sin(dlat/2)**2 + math.cos(math.radians(lat1)) * math.cos(math.radians(lat2)) * math.sin(dlon/2)**2
|
||||
return R * 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
|
||||
|
||||
def calculate_bearing(lat1, lon1, lat2, lon2):
|
||||
phi1 = math.radians(lat1)
|
||||
phi2 = math.radians(lat2)
|
||||
delta_lon = math.radians(lon2 - lon1)
|
||||
x = math.sin(delta_lon) * math.cos(phi2)
|
||||
y = math.cos(phi1)*math.sin(phi2) - math.sin(phi1)*math.cos(phi2)*math.cos(delta_lon)
|
||||
return (math.degrees(math.atan2(x, y)) + 360) % 360
|
||||
|
||||
def noisy_bearing(bearing, distance_km, min_noise=0.5, max_noise=10.0, max_effective_km=10.0):
|
||||
distance_km = min(distance_km, max_effective_km)
|
||||
noise = max_noise - (distance_km / max_effective_km) * (max_noise - min_noise)
|
||||
return (bearing + random.uniform(-noise, noise)) % 360
|
||||
|
||||
def generate_converging_spiral(center_lat, center_lon, num_turns=10, points_per_turn=100, max_radius_km=50):
|
||||
path = []
|
||||
total_points = num_turns * points_per_turn
|
||||
for i in range(total_points):
|
||||
angle = 2 * math.pi * (i / points_per_turn)
|
||||
radius_km = max_radius_km * (1 - i / total_points)
|
||||
lat_offset = (radius_km * math.sin(angle)) / 111
|
||||
lon_offset = (radius_km * math.cos(angle)) / (111 * math.cos(math.radians(center_lat)))
|
||||
path.append((center_lat + lat_offset, center_lon + lon_offset))
|
||||
return path
|
||||
|
||||
def generate_orbit(center_lat, center_lon, radius_km=0.1, num_points=100):
|
||||
path = []
|
||||
for i in range(num_points):
|
||||
angle = 2 * math.pi * i / num_points
|
||||
dlat = (radius_km * math.sin(angle)) / 111
|
||||
dlon = (radius_km * math.cos(angle)) / (111 * math.cos(math.radians(center_lat)))
|
||||
path.append((center_lat + dlat, center_lon + dlon))
|
||||
return path
|
||||
|
||||
search_path = generate_converging_spiral(center_lat, center_lon, points_per_turn=1000)
|
||||
|
||||
async def simulate():
|
||||
global orbit_active, orbit_path, orbit_index, current_lat, current_lon, active_orbit_target, active_freq, completed_orbits
|
||||
|
||||
point_index = 0
|
||||
|
||||
while True:
|
||||
try:
|
||||
while not commands.empty():
|
||||
cmd = await commands.get()
|
||||
if cmd.get("type") == "command" and cmd.get("action") == "orbit_target":
|
||||
cmd_freq = cmd.get("freq")
|
||||
if cmd_freq in completed_orbits:
|
||||
print(f"🚫 Skipping orbit for {cmd_freq} — already completed.")
|
||||
continue
|
||||
|
||||
if active_freq is None:
|
||||
orbit_queue.append(cmd)
|
||||
active_freq = cmd_freq
|
||||
elif not orbit_active and cmd_freq == active_freq:
|
||||
print(f"🔁 Updating active orbit target: {cmd}")
|
||||
active_orbit_target["lat"] = cmd["lat"]
|
||||
active_orbit_target["lon"] = cmd["lon"]
|
||||
active_orbit_target["radius"] = cmd.get("radius", 0.1)
|
||||
else:
|
||||
print(f"⏳ Queuing orbit for later: freq={cmd_freq}")
|
||||
orbit_queue.append(cmd)
|
||||
|
||||
except asyncio.QueueEmpty:
|
||||
pass
|
||||
|
||||
step_km = haversine(*search_path[point_index % len(search_path)], *search_path[(point_index + 1) % len(search_path)])
|
||||
|
||||
if not orbit_active and active_orbit_target is None:
|
||||
# Look for next valid orbit target
|
||||
while orbit_queue:
|
||||
next_cmd = orbit_queue.pop(0)
|
||||
freq = next_cmd.get("freq")
|
||||
if freq in completed_orbits:
|
||||
print(f"🚫 Skipping orbit for {freq} — already completed.")
|
||||
continue
|
||||
print(f"🛰️ Starting new orbit for {freq}")
|
||||
active_orbit_target = next_cmd
|
||||
active_freq = freq
|
||||
orbit_index = 0
|
||||
orbit_path = []
|
||||
break # found a valid one
|
||||
|
||||
|
||||
|
||||
if active_orbit_target:
|
||||
orbit_center_lat = active_orbit_target["lat"]
|
||||
orbit_center_lon = active_orbit_target["lon"]
|
||||
radius = active_orbit_target.get("radius", 0.1)
|
||||
dist = haversine(current_lat, current_lon, orbit_center_lat, orbit_center_lon)
|
||||
|
||||
if dist < 0.5:
|
||||
if not orbit_active:
|
||||
print(f"🛰️ Starting orbit around ({orbit_center_lat:.4f}, {orbit_center_lon:.4f})")
|
||||
orbit_center_lat = active_orbit_target["lat"]
|
||||
orbit_center_lon = active_orbit_target["lon"]
|
||||
radius = active_orbit_target.get("radius", 0.1)
|
||||
orbit_path = generate_orbit(orbit_center_lat, orbit_center_lon, radius_km=radius)
|
||||
orbit_index = 0
|
||||
orbit_active = True
|
||||
last_sent_orbit = {"lat": orbit_center_lat, "lon": orbit_center_lon}
|
||||
else:
|
||||
bearing_to_target = calculate_bearing(current_lat, current_lon, orbit_center_lat, orbit_center_lon)
|
||||
dlat = (step_km * math.cos(math.radians(bearing_to_target))) / 111
|
||||
dlon = (step_km * math.sin(math.radians(bearing_to_target))) / (111 * math.cos(math.radians(current_lat)))
|
||||
current_lat += dlat
|
||||
current_lon += dlon
|
||||
|
||||
if orbit_active and orbit_path:
|
||||
target_lat, target_lon = orbit_path[orbit_index % len(orbit_path)]
|
||||
orbit_index += 1
|
||||
current_lat = target_lat
|
||||
current_lon = target_lon
|
||||
|
||||
if orbit_index >= len(orbit_path):
|
||||
orbit_active = False
|
||||
print(f"✅ Finished orbit for {active_freq}")
|
||||
completed_orbits.add(active_freq)
|
||||
active_freq = None
|
||||
active_orbit_target = None
|
||||
elif not active_orbit_target:
|
||||
current_lat, current_lon = search_path[point_index % len(search_path)]
|
||||
point_index += 1
|
||||
|
||||
await broadcast(json.dumps({"type": "drone", "lat": current_lat, "lon": current_lon, "alt": 100}))
|
||||
|
||||
for target in targets:
|
||||
await asyncio.sleep(0.01)
|
||||
distance_km = haversine(current_lat, current_lon, target["lat"], target["lon"])
|
||||
if distance_km > 10:
|
||||
continue
|
||||
signal_strength = max(0.0, 1.0 - (distance_km / 10))
|
||||
bearing = calculate_bearing(current_lat, current_lon, target["lat"], target["lon"])
|
||||
noisy = noisy_bearing(bearing, distance_km)
|
||||
mirror = (noisy + 180) % 360
|
||||
await broadcast(json.dumps({
|
||||
"type": "bearing",
|
||||
"freq": target["freq"],
|
||||
"lat": current_lat,
|
||||
"lon": current_lon,
|
||||
"bearing": noisy,
|
||||
"mirror": mirror,
|
||||
"alt": 100,
|
||||
"weight": signal_strength,
|
||||
"color": FREQ_COLORS[target["freq"]]
|
||||
}))
|
||||
|
||||
async def websocket_handler(websocket):
|
||||
clients.add(websocket)
|
||||
print("🧪 Client connected")
|
||||
try:
|
||||
async for message in websocket:
|
||||
print("📥 Received:", message)
|
||||
try:
|
||||
data = json.loads(message)
|
||||
if data.get("type") == "command" and data.get("action") == "orbit_target":
|
||||
print("🛰️ Queuing orbit command:", data)
|
||||
await commands.put(data)
|
||||
except Exception as e:
|
||||
print("❌ Invalid message:", e)
|
||||
finally:
|
||||
clients.remove(websocket)
|
||||
|
||||
async def broadcast(message):
|
||||
if clients:
|
||||
await asyncio.gather(*(client.send(message) for client in clients))
|
||||
|
||||
async def main():
|
||||
print("📡 Simulation server running at ws://localhost:8765")
|
||||
server = websockets.serve(websocket_handler, "localhost", 8765)
|
||||
await asyncio.gather(server, simulate())
|
||||
|
||||
if __name__ == "__main__":
|
||||
asyncio.run(main())
|
||||
0
AUTOSAMPLERIND/v1/want){const
Normal file
0
AUTOSAMPLERIND/v1/want){const
Normal file
0
AUTOSAMPLERIND/v1/{const
Normal file
0
AUTOSAMPLERIND/v1/{const
Normal file
Reference in New Issue
Block a user