{"metadata":{"kernelspec":{"language":"python","display_name":"Python 3","name":"python3"},"language_info":{"name":"python","version":"3.10.13","mimetype":"text/x-python","codemirror_mode":{"name":"ipython","version":3},"pygments_lexer":"ipython3","nbconvert_exporter":"python","file_extension":".py"},"kaggle":{"accelerator":"none","dataSources":[{"sourceId":60095,"databundleVersionId":6542333,"sourceType":"competition"},{"sourceId":8158189,"sourceType":"datasetVersion","datasetId":4826259},{"sourceId":8167986,"sourceType":"datasetVersion","datasetId":4833549},{"sourceId":8168006,"sourceType":"datasetVersion","datasetId":4833565},{"sourceId":8168089,"sourceType":"datasetVersion","datasetId":4833621},{"sourceId":8168124,"sourceType":"datasetVersion","datasetId":4833649},{"sourceId":8168150,"sourceType":"datasetVersion","datasetId":4833669}],"dockerImageVersionId":30673,"isInternetEnabled":true,"language":"python","sourceType":"notebook","isGpuEnabled":false}},"nbformat_minor":4,"nbformat":4,"cells":[{"cell_type":"code","source":"import numpy as np \nimport pandas as pd \nimport os, csv, sys\nimport glob\nfrom tqdm.notebook import tqdm\nfrom dataclasses import dataclass\nfrom scipy.interpolate import InterpolatedUnivariateSpline\nfrom datetime import datetime, timezone, timedelta\n\n!cp -r ../input/navpylib/NavPy-master/* ./\nimport navpy\n!cp -r ../input/gnss-analysis-8/gnss-analysis-main/* ./\nfrom gnssutils import EphemerisManager\n\n#pip install georinex unlzw3 trước khi chạy\n# Input data files are available in the read-only \"../input/\" directory\n# For example, running this (by clicking run or pressing Shift+Enter) will list all files under the input directory\n# You can write up to 20GB to the current directory (/kaggle/working/) that gets preserved as output when you create a version using \"Save & Run All\" \n# You can also write temporary files to /kaggle/temp/, but they won't be saved outside of the current session","metadata":{"_uuid":"8f2839f25d086af736a60e9eeb907d3b93b6e0e5","_cell_guid":"b1076dfc-b9ad-4769-8c92-a6c4dae69d19","execution":{"iopub.status.busy":"2024-04-20T08:55:22.058380Z","iopub.execute_input":"2024-04-20T08:55:22.058842Z","iopub.status.idle":"2024-04-20T08:55:24.643237Z","shell.execute_reply.started":"2024-04-20T08:55:22.058806Z","shell.execute_reply":"2024-04-20T08:55:24.642006Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"INPUT_PATH = '../input/smartphone-decimeter-2023/sdc2023/train/2020-06-25-00-34-us-ca-mtv-sb-101/pixel4/supplemental/gnss_log.txt'\n#Đọc dữ liệu từ file gnss_log.txt. Ghi các dữ liệu vào measurement.\nwith open(INPUT_PATH) as csvfile:\n    reader = csv.reader(csvfile)\n    for row in reader:       \n        if '# Raw' in row:\n              measurement = [row[1:]]\n        else:\n            if 'Raw' in row:\n                measurement.append(row[1:])\nmeasurement = pd.DataFrame(measurement[1:], columns = measurement[0])","metadata":{"execution":{"iopub.status.busy":"2024-04-20T08:55:24.645891Z","iopub.execute_input":"2024-04-20T08:55:24.646364Z","iopub.status.idle":"2024-04-20T08:55:25.484754Z","shell.execute_reply.started":"2024-04-20T08:55:24.646321Z","shell.execute_reply":"2024-04-20T08:55:25.483747Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"#lọc các vệ tinh thuộc GPS\nmeasurement.loc[measurement['ConstellationType'] == '1', 'Constellation'] = 'G'\nmeasurement = measurement.loc[measurement['Constellation'] == 'G']\nmeasurement.loc[measurement['Svid'].str.len() == 1, 'Svid'] = '0' + measurement['Svid']\nmeasurement['SvName'] = measurement['Constellation'] + measurement['Svid']","metadata":{"execution":{"iopub.status.busy":"2024-04-20T08:55:25.486046Z","iopub.execute_input":"2024-04-20T08:55:25.486404Z","iopub.status.idle":"2024-04-20T08:55:25.634729Z","shell.execute_reply.started":"2024-04-20T08:55:25.486374Z","shell.execute_reply":"2024-04-20T08:55:25.633744Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"#chuyển giá trị của các cột về dạng số\nmeasurement['utcTimeMillis'] = pd.to_numeric(measurement['utcTimeMillis'])\nmeasurement['Cn0DbHz'] = pd.to_numeric(measurement['Cn0DbHz'])\nmeasurement['TimeNanos'] = pd.to_numeric(measurement['TimeNanos'])\nmeasurement['FullBiasNanos'] = pd.to_numeric(measurement['FullBiasNanos'])\nmeasurement['ReceivedSvTimeNanos']  = pd.to_numeric(measurement['ReceivedSvTimeNanos'])\nmeasurement['PseudorangeRateMetersPerSecond'] = pd.to_numeric(measurement['PseudorangeRateMetersPerSecond'])\nmeasurement['ReceivedSvTimeUncertaintyNanos'] = pd.to_numeric(measurement['ReceivedSvTimeUncertaintyNanos'])\nmeasurement['BiasNanos'] = pd.to_numeric(measurement['BiasNanos'])\nmeasurement['TimeOffsetNanos'] = pd.to_numeric(measurement['TimeOffsetNanos'])\nmeasurement['Svid'] = pd.to_numeric(measurement['Svid'])","metadata":{"execution":{"iopub.status.busy":"2024-04-20T08:55:25.637948Z","iopub.execute_input":"2024-04-20T08:55:25.638415Z","iopub.status.idle":"2024-04-20T08:55:25.874507Z","shell.execute_reply.started":"2024-04-20T08:55:25.638374Z","shell.execute_reply":"2024-04-20T08:55:25.873487Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"Tính thời gian gửi tại vệ tinh và thời gian nhận tại máy thu.\nThời gian nhận tại máy thu được tính bằng:\n\n****tRxNanos = TimeNanos - FullBiasNanos****\n\nThời gian nhận được tính từ mốc GPS epoch (6/1/1980). Thời gian gửi tại vệ tinh được tính theo từng tuần. Để đưa thời gian gửi về cùng mốc GPS epoch với thời gian nhận, ta tính số tuần GPS đã trôi qua (theo thời gian nhận), sau đó nhân với thời gian một tuần và cộng với 'ReceivedSvTimeNanos'.\n\n****GPSWeekNum = round(tRxNanos/WeekNanos)****\n\n****tTxNanos = (GPSWeekNum * WeekNanos) + ReceivedSvTimeNanos****\n\n*Chú ý: Đây là cách tính với riêng hệ thống GPS, với các hệ thống vệ tinh khác sẽ có cách tính khác*","metadata":{}},{"cell_type":"code","source":"WeekNanos = 604800000000000\n\nmeasurement['tRxNanos'] = measurement['TimeNanos'] - measurement['FullBiasNanos']\nmeasurement['GpsWeekNum'] = np.floor(measurement['tRxNanos']/WeekNanos).astype(int)\nmeasurement['tTxNanos'] = measurement['GpsWeekNum'] * WeekNanos + measurement['ReceivedSvTimeNanos']\nmeasurement['tRxSeconds'] = measurement['tRxNanos']/1e9\nmeasurement['tTxSeconds'] = measurement['tTxNanos']/1e9","metadata":{"execution":{"iopub.status.busy":"2024-04-20T08:55:25.876042Z","iopub.execute_input":"2024-04-20T08:55:25.876398Z","iopub.status.idle":"2024-04-20T08:55:25.888627Z","shell.execute_reply.started":"2024-04-20T08:55:25.876369Z","shell.execute_reply":"2024-04-20T08:55:25.887759Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"Tính pseudorange như sau: \n\n$Pseudorange = \\frac{tRx-tTx}{1e9} * c$","metadata":{}},{"cell_type":"code","source":"LightSpeed = 299792458\nmeasurement[\"PseudorangeMeters\"] = (measurement['tRxNanos'] - measurement['tTxNanos'])/1e9 * LightSpeed","metadata":{"execution":{"iopub.status.busy":"2024-04-20T08:55:25.889916Z","iopub.execute_input":"2024-04-20T08:55:25.890456Z","iopub.status.idle":"2024-04-20T08:55:25.901348Z","shell.execute_reply.started":"2024-04-20T08:55:25.890427Z","shell.execute_reply":"2024-04-20T08:55:25.900477Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"Tiếp theo cần phải tính các tọa độ của vệ tinh. Để tính được, cần lấy các thông số ephemeris của vệ tinh. Sử dụng thư viện gnss analysis để lấy dữ liệu ephemeris của các vệ tinh từ International GNSS Service (IGS). Broadcast ephemerides có hiệu lực trong 4h kể từ lúc được phát, vậy nên hàm EphemerisManager sẽ tải về dữ liệu trong một ngày của vệ tinh, sau đó trả về các tham số mới nhất ứng với thời gian yêu cầu tín hiệu từ vệ tinh.","metadata":{}},{"cell_type":"code","source":"#chuyển giá trị thời gian về dạng date time\nmeasurement['UnixTime'] = pd.to_datetime(measurement['utcTimeMillis'],unit='ms', utc=True)\n#Chia measurement thành các epoch, mỗi epoch gồm các vệ tinh có cùng thời gian thu\nmeasurement['Epoch'] = 0\nmeasurement.loc[measurement['UnixTime'] - measurement['UnixTime'].shift() > timedelta(milliseconds=200), 'Epoch'] = 1\nmeasurement['Epoch'] = measurement['Epoch'].cumsum()","metadata":{"execution":{"iopub.status.busy":"2024-04-20T08:55:25.902616Z","iopub.execute_input":"2024-04-20T08:55:25.903114Z","iopub.status.idle":"2024-04-20T08:55:25.923445Z","shell.execute_reply.started":"2024-04-20T08:55:25.903087Z","shell.execute_reply":"2024-04-20T08:55:25.922434Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"Tính tọa độ cho một epoch đầu tiên","metadata":{}},{"cell_type":"code","source":"#lấy dữ liệu ephemeris\nephemeris_data_directory = '../output/kaggle/working/'\nmanager = EphemerisManager(ephemeris_data_directory)\n\nepoch = 0\nnum_sat = 0\n\nwhile num_sat < 5:\n    one_epoch = measurement.loc[measurement['Epoch'] == epoch].drop_duplicates(subset='SvName')\n    timestamp = one_epoch.iloc[0]['UnixTime'].to_pydatetime(warn=False)\n    one_epoch.set_index('SvName',inplace=True)\n    num_sat = len(one_epoch.index)\n    epoch += 1\n    \nsatellites = one_epoch.index.unique().tolist()\nephemeris = manager.get_ephemeris(timestamp, satellites)","metadata":{"execution":{"iopub.status.busy":"2024-04-20T08:55:25.924736Z","iopub.execute_input":"2024-04-20T08:55:25.925051Z","iopub.status.idle":"2024-04-20T08:55:28.058277Z","shell.execute_reply.started":"2024-04-20T08:55:25.925023Z","shell.execute_reply":"2024-04-20T08:55:28.057100Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"SvPositionPath = '../input/smartphone-decimeter-2023/sdc2023/train/2020-06-25-00-34-us-ca-mtv-sb-101/pixel4/device_gnss.csv'\ndef calculate_satellite_position(timestamp):\n    df = pd.read_csv(SvPositionPath)\n    sv_position = pd.DataFrame()  # Chuyển từ danh sách sang từ điển để lưu vị trí của vệ tinh\n    selected_rows = df.loc[df['utcTimeMillis'] == timestamp]\n    selected_rows = selected_rows.loc[selected_rows['ConstellationType'] == 1].drop_duplicates(subset='Svid')\n    for row in selected_rows:\n        sv_position['x_k'] = selected_rows['SvPositionXEcefMeters']\n        sv_position['y_k'] = selected_rows['SvPositionYEcefMeters']\n        sv_position['z_k'] = selected_rows['SvPositionZEcefMeters']\n        sv_position['SvClockDriftMetersPerSecond'] = selected_rows['SvClockDriftMetersPerSecond']\n        sv_position['IonosphericDelayMeters'] = selected_rows['IonosphericDelayMeters']   \n        sv_position['TroposphericDelayMeters'] = selected_rows['TroposphericDelayMeters']\n        sv_position['Svid'] = selected_rows['Svid']\n        sv_position['SvClockBiasMeters'] = selected_rows['SvClockBiasMeters']\n    return sv_position\ntimestamp = one_epoch.iloc[1]['utcTimeMillis']\nsv_position = calculate_satellite_position(timestamp)\none_epoch.set_index('Svid',inplace=True)\nsv_position.set_index('Svid',inplace=True)\nprint(sv_position)","metadata":{"execution":{"iopub.status.busy":"2024-04-20T08:56:51.808078Z","iopub.execute_input":"2024-04-20T08:56:51.808544Z","iopub.status.idle":"2024-04-20T08:56:52.388399Z","shell.execute_reply.started":"2024-04-20T08:56:51.808511Z","shell.execute_reply":"2024-04-20T08:56:52.386988Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"#initial guesses of receiver clock bias and position\nb0 = 0\nx0 = np.array([0, 0, 0])\nxs = sv_position[['x_k', 'y_k', 'z_k']].to_numpy()\n\n# Apply satellite clock bias to correct the measured pseudorange values\npr = one_epoch['PseudorangeMeters'] + sv_position['IonosphericDelayMeters'] + sv_position['TroposphericDelayMeters'] + sv_position['SvClockBiasMeters'] \n#pr = one_epoch['PseudorangeMeters']\npr = pr.to_numpy()\nprint(pr)","metadata":{"execution":{"iopub.status.busy":"2024-04-20T08:56:56.566147Z","iopub.execute_input":"2024-04-20T08:56:56.566571Z","iopub.status.idle":"2024-04-20T08:56:56.578752Z","shell.execute_reply.started":"2024-04-20T08:56:56.566540Z","shell.execute_reply":"2024-04-20T08:56:56.577183Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def least_squares(xs, measured_pseudorange, x0, b0):\n    dx = 100*np.ones(3)\n    b = b0\n    # set up the G matrix with the right dimensions. We will later replace the first 3 columns\n    # note that b here is the clock bias in meters equivalent, so the actual clock bias is b/LIGHTSPEED\n    G = np.ones((measured_pseudorange.size, 4))\n    iterations = 0\n    while np.linalg.norm(dx) > 1e-3:\n        # Eq. (2):\n        r = np.linalg.norm(xs - x0, axis=1)\n        # Eq. (1):\n        phat = r + b0\n        # Eq. (3):\n        deltaP = measured_pseudorange - phat\n        G[:, 0:3] = -(xs - x0) / r[:, None]\n        # Eq. (4):\n        sol = np.linalg.inv(np.transpose(G) @ G) @ np.transpose(G) @ deltaP\n        # Eq. (5):\n        dx = sol[0:3]\n        db = sol[3]\n        x0 = x0 + dx\n        b0 = b0 + db\n    norm_dp = np.linalg.norm(deltaP)\n    return x0, b0, norm_dp\n\nx, b, dp = least_squares(xs, pr, x0, b0)\nprint(navpy.ecef2lla(x))\n","metadata":{"execution":{"iopub.status.busy":"2024-04-20T08:57:43.627075Z","iopub.execute_input":"2024-04-20T08:57:43.627483Z","iopub.status.idle":"2024-04-20T08:57:43.642419Z","shell.execute_reply.started":"2024-04-20T08:57:43.627454Z","shell.execute_reply":"2024-04-20T08:57:43.640827Z"},"trusted":true},"execution_count":null,"outputs":[]}]}