143 lines
7.3 KiB
HTML
143 lines
7.3 KiB
HTML
<!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>
|