CIR_Stat.py

# -*- coding: utf-8 -*-
"""
Created on Mon Jan 19 21:28:16 2026

@author: Ray
"""

import numpy as np
import matplotlib.pyplot as plt
import os
from process_data import parse_dataset_file
from cdf import plot_cdf_of_errors
from utils import is_position_jump_too_large,is_within_bounds
env = 'pocket'
# env = 'chest'
data = parse_dataset_file(f"dataset/{env}_5HZ.txt")
last_d = np.zeros(3, dtype=float)
count = 0
poll_cnt =0
flag_a = 1
positionPre = None
clear_positions = []   
gt_positions = []       
round_ids = []        
F_xy = None

anchor_positions = [
    (0.0, 4.0),
    (6.4, 0.0),
    (6.4, 8.0),
    (0.0, 0.0),
    (6.4, 4.0),
    (0.0, 8.0)
]

def extract_round_arrays(round_data, anchor_num):
    features = {
        "Ddoa":      np.full(anchor_num, np.inf),
        "tof_delta": np.full(anchor_num, np.inf),
        "rssi":      np.full(anchor_num, np.inf),
        "kurt":      np.full(anchor_num, np.inf),
        "rms":       np.full(anchor_num, np.inf),
        "energy_db": np.full(anchor_num, np.inf),
    }

    for aid, feat in round_data["anchors"].items():
        idx = aid - 1
        for key in features:
            if key in feat:
                features[key][idx] = feat[key]

    info = {}
    if round_data.get("cluster"):
        info["X"] = round_data["cluster"]["x"]
        info["Y"] = round_data["cluster"]["y"]

    return features, info

def Chan(distance_diff, anchor_positions):
    global positionPre

    if len(anchor_positions) < 3 or len(distance_diff) < 2:
        return 0

    anchor_positions = np.array(anchor_positions)
    main_anchor = anchor_positions[0]    
    valid_indices = [i for i, d in enumerate(distance_diff) if abs(d) < 99]
    if len(valid_indices) < 2:
        return 0

    d = [distance_diff[i] for i in valid_indices]
    ref_anchors = anchor_positions[1:][valid_indices]
    used_anchors = np.vstack([main_anchor, ref_anchors])

    dx1 = used_anchors[1][0] - main_anchor[0]
    dx2 = used_anchors[2][0] - main_anchor[0]
    dy1 = used_anchors[1][1] - main_anchor[1]
    dy2 = used_anchors[2][1] - main_anchor[1]

    K1 = main_anchor[0]**2 + main_anchor[1]**2
    K2 = used_anchors[1][0]**2 + used_anchors[1][1]**2
    K3 = used_anchors[2][0]**2 + used_anchors[2][1]**2

    det = dx2 * dy1 - dx1 * dy2
    if det == 0:
        return 0

    P1 = np.array([[dy2 / det, -dy1 / det], [-dx2 / det, dx1 / det]])
    P2 = np.array([d[0], d[1]])
    P3 = np.array([0.5 * (K1 + d[0]**2 - K2), 0.5 * (K1 + d[1]**2 - K3)])

    A1 = main_anchor
    PP12 = P1 @ P2
    PP13 = P1 @ P3 - A1

    a = np.dot(PP12, PP12) - 1
    b = 2 * np.dot(PP12, PP13)
    c = np.dot(PP13, PP13)

    discriminant = b**2 - 4 * a * c
    if discriminant < 0:
        return 0

    R1 = (-b - np.sqrt(discriminant)) / (2 * a)
    positionPre = PP12 * R1 + PP13 + A1
    
    return 1

def NLS(distance_diff, anchor_positions):
    
    global positionPre
    
    if len(anchor_positions) < 3 or len(distance_diff) < 2:
        return 0

    anchor_positions = np.array(anchor_positions)
    main_anchor = anchor_positions[0]    
    valid_indices = [i for i, d in enumerate(distance_diff) if abs(d) < 99]
    if len(valid_indices) < 2:
        return 0

    d = [distance_diff[i] for i in valid_indices]
    ref_anchors = anchor_positions[1:][valid_indices]
    used_anchors = np.vstack([main_anchor, ref_anchors])
    
    positionCur = np.zeros(2)

    diff = 1
    cnt = 0
    threshold = 1e-2
    # max_iter = 4
    total_anchors = len(d) + 1

    while diff > threshold and cnt < 3:
        e = np.zeros((total_anchors, 2))
        h = np.zeros((total_anchors, 3))
        h1 = np.zeros((3, total_anchors))
        Y = np.zeros(total_anchors)

        for i in range(total_anchors):
            e[i] = positionPre - used_anchors[i]
            norm = np.linalg.norm(e[i])
            if norm == 0:
                return 0
            h[i, :2] = e[i] / norm
            h[i, 2] = -1
            h1[:, i] = h[i]

        H = h1 @ h
        try:
            H_inv = np.linalg.inv(H)
        except np.linalg.LinAlgError:
            return 0

        for i in range(total_anchors):
            Y[i] = h[i, 0] * used_anchors[i][0] + h[i, 1] * used_anchors[i][1] + (d[i - 1] if i > 0 else 0)

        result = H_inv @ h1 @ Y
        positionCur[:2] = result[:2]
        diff = np.sum((positionCur - positionPre) ** 2)
        positionPre = positionCur.copy()
        cnt += 1

    return 1

def Wtaylor(distance_diff, anchor_positions, rssi_array, delta_array,lastposition, last_d):
    global positionPre

    if len(anchor_positions) < 3 or len(distance_diff) < 2:
        return 0

    anchor_positions = np.array(anchor_positions)
    rssi_array = np.array(rssi_array, dtype=float)
    delta_array = np.array(delta_array, dtype=float)
    main_anchor = anchor_positions[0]

    valid_indices = [
        i for i, d in enumerate(distance_diff)
        if abs(d) < 99 and rssi_array[i] != -999.99 and abs(delta_array[i]) < 999
    ]
    if len(valid_indices) < 2:
        return 0

    d = [distance_diff[i] for i in valid_indices]
    # rssi_valid = [rssi_array[i] for i in valid_indices]
    # delta_valid = [delta_array[i] for i in valid_indices]

    valid_anchors = [anchor_positions[i + 1] for i in valid_indices]  
    valid_anchors.insert(0, main_anchor)

    position_cur = np.array(lastposition)
    delta = np.array([1.0, 1.0])
    iter_num = 0


    # rssi = np.array(rssi_valid)
    # delta = np.abs(delta_valid)
    
    # # RSSI gating (data-adaptive)
    # mu_r = np.median(rssi)
    # sigma_r = np.median(np.abs(rssi - mu_r)) + 1e-6
    # rssi_gate = 1.0 / (1.0 + np.exp(-(rssi - mu_r) / sigma_r))

    # delta_mean = np.mean(delta) + 1e-6
    # delta_gate = np.exp(-delta / delta_mean)

    # combined_weight = rssi_gate * delta_gate
    # combined_weight /= np.sum(combined_weight)


    # Q = np.diag(1.0 / combined_weight)
    Q = np.diag([0.01] * len(d)) 

    while np.abs(delta).sum() > 0.01 and iter_num < 3:
        iter_num += 1
        distances = np.array([np.linalg.norm(position_cur - anchor) for anchor in valid_anchors])

        hi = np.array([
            d[i] - (distances[i + 1] - distances[0])
            for i in range(len(d))
        ])

        Gi = np.zeros((len(d), 2))
        for i in range(len(d)):
            Gi[i, 0] = (position_cur[0] - valid_anchors[i + 1][0]) / distances[i + 1] - \
                       (position_cur[0] - valid_anchors[0][0]) / distances[0]
            Gi[i, 1] = (position_cur[1] - valid_anchors[i + 1][1]) / distances[i + 1] - \
                       (position_cur[1] - valid_anchors[0][1]) / distances[0]

        try:
            GiT_Qinv_Gi = Gi.T @ np.linalg.inv(Q) @ Gi
            GiT_Qinv_hi = Gi.T @ np.linalg.inv(Q) @ hi
            delta = np.linalg.solve(GiT_Qinv_Gi, GiT_Qinv_hi)
        except np.linalg.LinAlgError:
            return 0

        position_cur += delta
        positionPre = position_cur.copy()

    return 1

def detect_nlos_anchor_valid_only(
    kurt_array,
    rms_array,
    energy_array,
    distance_diff,
    d_th=99
):

    kurt_array   = np.asarray(kurt_array, dtype=float)
    rms_array    = np.asarray(rms_array, dtype=float)
    energy_array = np.asarray(energy_array, dtype=float)
    distance_diff = np.asarray(distance_diff, dtype=float)


    valid_indices = [i for i, d in enumerate(distance_diff) if abs(d) < d_th]
    if len(valid_indices) < 2:
        return None, None


    kurt_v   = kurt_array[valid_indices]
    rms_v    = rms_array[valid_indices]
    energy_v = energy_array[valid_indices]


    f_kurt   = -kurt_v
    f_rms    =  rms_v
    f_energy = -energy_v


    def robust_z(x):
        med = np.median(x)
        mad = np.median(np.abs(x - med)) + 1e-6
        return (x - med) / mad

    z_kurt   = robust_z(f_kurt)
    z_rms    = robust_z(f_rms)
    z_energy = robust_z(f_energy)

    nlos_score_valid = np.sqrt(
        z_kurt**2 +
        z_rms**2 +
        z_energy**2
    )

    nlos_score_full = np.full(len(kurt_array), -np.inf)
    for idx, s in zip(valid_indices, nlos_score_valid):
        nlos_score_full[idx] = s

    nlos_idx = int(np.argmax(nlos_score_full))

    return nlos_idx, nlos_score_full




unvaild = 0

for round_index, round_data in data.items():

    features, info = extract_round_arrays(round_data, len(anchor_positions)-1)
    
    d_array     = features["Ddoa"]
    delta_array = features["tof_delta"]
    rssi_array  = features["rssi"]
    kurt_array  = features["kurt"]
    rms_array   = features["rms"]
    energy_array = features["energy_db"]
    if last_d[0] == 0:
        last_d = d_array
    size = np.sum((d_array < 100) & np.isfinite(d_array))
    poll_cnt = poll_cnt + 1
    taylor_result_valid = 0
    update_flag = 0
    if 'X' in info and 'Y' in info:
        gt_positions.append([info['X'], info['Y']])
        
    remaining_iterations  = len(anchor_positions) - 2;  
    
    while size >= 2 and remaining_iterations > 0:
        Chan_flag = Chan(d_array, anchor_positions)
        if F_xy is None:
            F_xy = positionPre
            
        if Chan_flag:
            Chan_valid = is_position_jump_too_large(positionPre,F_xy,max_distance=np.sqrt(8.0 + poll_cnt * poll_cnt * 0.8))
            if  Chan_valid or is_within_bounds(positionPre, anchor_positions, margin=1.0):
                lastposition = positionPre
                chan_result_valid = 1
            else:
                chan_result_valid = 0
                
        NLS_flag = NLS(d_array, anchor_positions)
        if NLS_flag:
            LS_valid = is_position_jump_too_large(positionPre,F_xy,max_distance=np.sqrt(8.0 + poll_cnt * poll_cnt * 0.8))
            if  LS_valid or is_within_bounds(positionPre, anchor_positions, margin=1.0):
                lastposition = positionPre
                nls_result_valid = 1
            else:
                nls_result_valid = 0   
                
        if chan_result_valid and nls_result_valid:
            Taylor_flag = Wtaylor(d_array, anchor_positions, rssi_array, delta_array,lastposition, last_d)
            taylor_result_valid = is_position_jump_too_large(positionPre,F_xy,max_distance=np.sqrt(8.0 + poll_cnt * poll_cnt * 0.8))
                
        else:
            nlos_idx, score = detect_nlos_anchor_valid_only(
                kurt_array,
                rms_array,
                energy_array,
                d_array  
            )
            print(f"nlos_idx:{nlos_idx}")
            if nlos_idx is not None:
                d_array[nlos_idx] = 65535.0            
                
        remaining_iterations = remaining_iterations - 1
        size = np.sum((d_array < 100) & np.isfinite(d_array))
        if taylor_result_valid == 1:
            update_flag = 1
            break   
                
    if update_flag == 1:
        if flag_a == 1:
            flag_a = 0  
            old_x = positionPre[0]
            old_y = positionPre[1]
                    
        x_diff = abs(positionPre[0] - old_x)
        y_diff = abs(positionPre[1] - old_y)
        
        bound_value = 1.3 + (0.1 * poll_cnt  * poll_cnt);
        if y_diff < bound_value and x_diff < bound_value:
            if x_diff < bound_value-0.2 and y_diff  < bound_value-0.2:
                poll_cnt = 0
                count = count + 1
                clear_positions.append(positionPre.copy())
                old_x = positionPre[0]
                old_y = positionPre[1]
                cnt = 0
            else:
                cnt = cnt + 1
                if cnt == 3:
                    old_x = positionPre[0]
                    old_y = positionPre[1]
                    cnt = 0 
            F_xy = positionPre
            
    for i in range(len(d_array)):
        d = d_array[i]
        rssi = rssi_array[i]
        if d > 200 or rssi < -200:
            continue  
        last_d[i] = d
        
print(count)

clear_x = [p[0] for p in clear_positions]
clear_y = [p[1] for p in clear_positions]

true_x = [p[0] for p in gt_positions]
true_y = [p[1] for p in gt_positions]


anchors = np.array(anchor_positions)

clear_positions = np.array(clear_positions)
gt_positions = np.array(gt_positions)



plt.rcParams["font.family"] = "Times New Roman"
save_dir = os.path.join(os.getcwd(), "results_data")
os.makedirs(save_dir, exist_ok=True)



save_dir = os.path.join(os.getcwd(), "results_data")
os.makedirs(save_dir, exist_ok=True)

save_path = os.path.join(save_dir, f"CIR_Stat_{env}.csv")
np.savetxt(save_path, np.column_stack((clear_x, clear_y)),
            delimiter=",", header="x,y", comments="")

print(f"saved in: {save_path}")

plt.figure(figsize=(6, 4), dpi=600)

plot_cdf_of_errors(true_x, true_y, label="Proposed", color='r')
plot_cdf_of_errors(clear_x, clear_y, label="CIR_Stat", color='b')

plt.xlabel("Error (meters)", fontsize=14)
plt.ylabel("CDF", fontsize=14)
plt.xlim(0, 1)
plt.ylim(0, 1)
plt.xticks(fontsize=12)
plt.yticks(fontsize=12)
plt.title("CDF of Localization Errors", fontsize=14, pad=10)
plt.legend(loc="best", fontsize=12, frameon=False)
plt.grid(True, which='both', linestyle='--', linewidth=0.5, alpha=0.7)
plt.tight_layout()

plt.show()


plt.figure(figsize=(3.8, 4.8))

plt.scatter(clear_positions[:, 0], clear_positions[:, 1],
            s=10, label="Chan Estimate")

plt.scatter(gt_positions[:, 0], gt_positions[:, 1],
            s=10, label="final Estimate")

plt.xlabel("X (m)")
plt.ylabel("Y (m)")

plt.xlim(0, 7)
plt.ylim(0, 8)

plt.grid(True)
plt.legend()
plt.show()