<jupyter_start><jupyter_text>Libraries1. [filterpy](https://filterpy.readthedocs.io/en/latest/kalman/UnscentedKalmanFilter.html)<jupyter_code>import numpy as np
import pandas as pd
import random
from glob import glob
import os
import matplotlib.pyplot as plt
from tqdm.notebook import tqdm
from pathlib import Path
import plotly.express as px
import seaborn as sns

import geopy
import pymap3d as pm
from filterpy.kalman import UnscentedKalmanFilter, MerweScaledSigmaPoints

import torch
import torch.nn as nn
import torch.nn.functional as F
import torch.optim as optim
from torch.utils.data import TensorDataset, DataLoader
import torchsummary
from sklearn.model_selection import train_test_split
from sklearn.preprocessing import StandardScaler, MinMaxScaler

import warnings
warnings.filterwarnings(action='ignore')<jupyter_output><empty_output><jupyter_text>Hyper Parameters<jupyter_code>SEED = 1990
random.seed(SEED)
np.random.seed(SEED)<jupyter_output><empty_output><jupyter_text>Useful functions<jupyter_code>def calc_haversine(lat1, lon1, lat2, lon2):
    lat1, lon1, lat2, lon2 = map(np.radians, [lat1, lon1, lat2, lon2])
    dlat = lat2 - lat1
    dlon = lon2 - lon1
    a = np.sin(dlat / 2.0)**2 + np.cos(lat1) * np.cos(lat2) * np.sin(dlon / 2.0)**2
    
    c = 2 * np.arcsin(a ** 0.5)
    dist = 6_367_000 * c
    return dist
def check_score(input_df: pd.DataFrame) -> pd.DataFrame:
    output_df = input_df.copy()
    
    output_df['meter'] = input_df.apply(
        lambda r: calc_haversine(
            r.latDeg, r.lngDeg, r.t_latDeg, r.t_lngDeg
        ),
        axis=1
    )

    meter_score = output_df['meter'].mean()

    scores = []
    for phone in output_df['phone'].unique():
        _index = output_df['phone']==phone
        p_50 = np.percentile(output_df.loc[_index, 'meter'], 50)
        p_95 = np.percentile(output_df.loc[_index, 'meter'], 95)
        scores.append(p_50)
        scores.append(p_95)

    score = sum(scores) / len(scores)
    
    return output_df, meter_score , score
ell_wgs84 = pm.Ellipsoid()
def calc_geo2enu(df:pd.DataFrame)->pd.DataFrame:
    output = df.copy()
    llh = np.array(df[['latDeg', 'lngDeg', 'heightAboveWgs84EllipsoidM']])
    denu = pm.geodetic2enu(llh[:,0], llh[:,1], llh[:,2], llh[0,0], llh[0,1], llh[0,2], ell=ell_wgs84)
    output['x'] = denu[0]
    output['y'] = denu[1]
    output['z'] = denu[2]
    
    return output

def calc_enu2geo(df:pd.DataFrame)->pd.DataFrame:
    output = df.copy()
    enu = np.array(df[['x', 'y', 'z']])
    llh = np.array(df[['latDeg', 'lngDeg', 'heightAboveWgs84EllipsoidM']])
    geo = pm.enu2geodetic(enu[:,0], enu[:,1], enu[:,2], llh[0,0], llh[0,1], llh[0,2], ell=ell_wgs84, deg = True)
    output['latDeg'] = geo[0]
    output['lngDeg'] = geo[1]
    output['heightAboveWgs84EllipsoidM'] = geo[2]
    
    return output<jupyter_output><empty_output><jupyter_text>Data<jupyter_code>data_dir = Path("../input/google-smartphone-decimeter-challenge")
df_train = pd.read_pickle(str(data_dir / "gsdc_extract_train.pkl.gzip"))
df_test = pd.read_pickle(str(data_dir / "gsdc_extract_test.pkl.gzip"))
phones = df_train['phone'].unique()
phone = phones[random.randint(0, len(phones))]
df_sample = df_train[df_train['phone'] == phone].copy().reset_index().drop(columns = ['index'])
print(df_sample.shape)
df_sample.head()
df_train.fillna(0, inplace = True)
df_test.fillna(0, inplace = True)
for col in df_train.columns:
    print(col)
features = ['latDeg',
            'lngDeg',
            'xSatPosM_Scaled',
            'ySatPosM_Scaled',
            'zSatPosM_Scaled',
            'xSatVelMps_Scaled',
            'ySatVelMps_Scaled',
            'zSatVelMps_Scaled',
            'GPS_L1', 
            'GPS_L5', 
            'GAL_E1', 
            'GAL_E5A', 
            'GLO_G1', 
            'BDS_B1I', 
            'BDS_B1C', 
            'BDS_B2A', 
            'QZS_J1', 
            'QZS_J5', 
           ]<jupyter_output><empty_output><jupyter_text>Kalman Filter Define$$\begin{matrix}x_t =& x_{t-1} + \frac{v_{t-1}}{w_{t-1}}\left({\sin}\left({\omega}_{t-1}dt + {\theta}_{t-1}\right) - {\sin}\left({\theta}\right)\right)\\y_t =& y_{t-1} + \frac{v_{t-1}}{w_{t-1}}\left({\cos}\left({\theta}_{t-1}\right) - {\cos}\left({\omega}_{t-1}dt + {\theta}_{t-1}\right)\right)\\v_t =& v_{t-1}\\{\theta}_t =& {\theta}_{t-1} + {\omega}_{t-1}dt\\{\omega}_t =& {\omega}_{t-1}\end{matrix}$$<jupyter_code>def fx(x, dt):
    xout = np.zeros_like(x)
    if abs(x[4]) > 1e-3:
        xout[0] = x[0] + x[2]/x[4] * (np.sin(x[4] * dt + x[3]) - np.sin(x[3]))
        xout[1] = x[1] + x[2]/x[4] * (np.cos(x[3]) - np.cos(x[4] * dt + x[3]))
        xout[2] = x[2]
        xout[3] = x[3] + x[4] * dt
        xout[4] = x[4]
    else:
        xout[0] = x[0] + x[2] * dt * (np.cos(x[3]))
        xout[1] = x[1] + x[2] * dt * (np.sin(x[3]))
        xout[2] = x[2]
        xout[3] = x[3] + x[4] * dt
        xout[4] = x[4]
        
    return xout

def hx(x):
    return x[[0,1,3,4]]
points = MerweScaledSigmaPoints(5, alpha = .1, beta = 2., kappa = -1)<jupyter_output><empty_output><jupyter_text>ANN Define<jupyter_code>class NoiseNetwork(nn.Module):
    def __init__(self, features):
        super().__init__()
        
        self.fc1 = nn.Linear(features, 256)
        self.fc2 = nn.Linear(256, 512)
        self.fc3 = nn.Linear(512, 256)
        self.fc_Q = nn.Linear(256, 25)
        self.fc_R = nn.Linear(256, 16)
        self.fc4 = nn.Linear(256, 41)
        self.fc_err1 = nn.Linear(41, 16)
        self.fc_err2 = nn.Linear(16, 1)
        
        self.bn1 = nn.BatchNorm1d(256)
        self.bn2 = nn.BatchNorm1d(512)
        self.bn3 = nn.BatchNorm1d(256)
        self.bn4 = nn.BatchNorm1d(41)
        
        self.drop03 = nn.Dropout(0.3)
        self.drop05 = nn.Dropout(0.5)
        self.drop06 = nn.Dropout(0.6)
        self.drop09 = nn.Dropout(0.9)
        
    def forward(self, x):
        x = self.drop09(x)
        x = self.fc1(x)
        x = F.relu(x)
        x = self.bn1(x)
        
        x = self.drop06(x)
        x = self.fc2(x)
        x = F.relu(x)
        x = self.bn2(x)
        
        x = self.drop05(x)
        x = self.fc3(x)
        x = F.relu(x)
        x = self.bn3(x)
        
        Q = self.drop03(x)
        Q = self.fc_Q(Q)
        
        R = self.drop03(x)
        R = self.fc_R(R)
        
        catQR = torch.cat([Q, R], axis = 1)
        
        x = self.drop03(x)
        x = self.fc4(x)
        x = F.relu(x)
        x = self.bn4(x)
        x = x + catQR
        
        x = self.drop03(x)
        x = self.fc_err1(x)
        x = F.relu(x)
        
        x = self.drop03(x)
        x = self.fc_err2(x)
        err = F.relu(x)
        
        Q = Q.reshape(-1, 5,5)
        Q = torch.matmul(Q.transpose(1,2), Q)
        
        R = R.reshape(-1, 4,4)
        R = torch.matmul(R.transpose(1,2), R)
        
        return Q, R, err

class KFLoss(nn.Module):
    def __init__(self, lamb = 0.99):
        super().__init__()
        self.loss = nn.SmoothL1Loss()
        self.lamb = lamb
        pass
    def forward(self, err_est, err):
        loss = self.lamb * self.loss(err_est, err) + (1 - self.lamb) * torch.abs(err_est).mean()
        return loss
    
model = NoiseNetwork(len(features))
optimizer = optim.SGD(model.parameters(), lr = 0.0001)
loss_func = KFLoss()
def batch_forward(df:pd.DataFrame):
    data = torch.Tensor(df[features].values)
    Q, R, err_est = model(data)
    
    return Q, R, err_est

def batch_filter(df_:pd.DataFrame, Q, R):
    df = df_.copy()
    df = calc_geo2enu(df)
    features = ['x','y','yawRad','UncalGyroZRadPerSec']
    df['yawRad'] = np.deg2rad(df['yawDeg'])
    meas = df[features]
    meas = meas.fillna(0)
    
    kf = UnscentedKalmanFilter(dim_x = 5, dim_z = 4, dt = 1, fx = fx, hx = hx, points = points)
    mu, cov = kf.batch_filter(meas.values, R)
    (xs, Ps, Ks) = kf.rts_smoother(mu, cov, Q)
    
    df['x'] = xs[:,0]
    df['y'] = xs[:,1]
    df['yawDeg'] = np.rad2deg(xs[:,2])
    df['UncalGyroZRadPerSec'] = xs[:,3]
    
    df = calc_enu2geo(df)
    return df
    
def batch_estim(df_:pd.DataFrame, train = True):
    
    Q, R, err_est = batch_forward(df_)
    Q = Q.detach().numpy()
    R = R.detach().numpy()
    
    df = batch_filter(df_, Q, R)
        
    return df, err_est, Q, R
def train(df_:pd.DataFrame):
    model.train()  # 신경망을 학습 모드로 전환
    phones = df_['phone'].unique()
    output = df_[['phone', 'millisSinceGpsEpoch']].copy()
    
    df_list = []
    for phone in tqdm(phones):
        df_sample = df_[df_['phone'] == phone]
        
        optimizer.zero_grad()  # 경사를 0으로 초기화
        
        df_sample, err_est, _, _ = batch_estim(df_sample)
        
        df_sample, mean, score = check_score(df_sample)
        loss = loss_func(err_est, torch.Tensor(df_sample['meter'].values).reshape(-1,1))
        df_list.append(df_sample)
        
        
        loss.backward()  # 오차를 역전파 계산
        optimizer.step()  # 역전파 계산한 값으로 가중치를 수정
    
    df_list = pd.concat(df_list)
    output = output.merge(df_list, on = ['phone', 'millisSinceGpsEpoch'])
    
    return output
        
def valid(df_:pd.DataFrame):
    model.eval()  # 신경망을 추론 모드로 전환
    phones = df_['phone'].unique()
    output = df_[['phone', 'millisSinceGpsEpoch']].copy()
    
    df_list = []
    
    with torch.no_grad():  # 추론 과정에는 미분이 필요없음
        for phone in phones:
            df_sample = df_[df_['phone'] == phone]

            df_sample, err_est, _, _ = batch_estim(df_sample)
            df_list.append(df_sample)
        
    df_list = pd.concat(df_list)
    output = output.merge(df_list, on = ['phone', 'millisSinceGpsEpoch'])
    
    return output
        
def test(df_:pd.DataFrame):
    model.eval()  # 신경망을 추론 모드로 전환
    phones = df_['phone'].unique()
    output = df_[['phone', 'millisSinceGpsEpoch']].copy()
    
    df_list = []
    
    with torch.no_grad():  # 추론 과정에는 미분이 필요없음
        for phone in phones:
            df_sample = df_[df_['phone'] == phone]

            df_sample, err_est, _, _ = batch_estim(df_sample, train = False)
            df_list.append(df_sample)
        
    df_list = pd.concat(df_list)
    output = output.merge(df_list, on = ['phone', 'millisSinceGpsEpoch'])
    
    return output
        
# sample_phone = np.random.choice(phones, 3, replace = False)
# sample_index = df_train['phone'].apply(lambda x: x in sample_phone)
# df_samples = df_train[sample_index]

for epoch in tqdm(range(100)):
    train_result = train(df_train)
    train_result, train_mean, train_score = check_score(train_result)
    
    print(f"{epoch + 1}: mean={train_mean}, score={train_score}")
valid_result = valid(df_test)
valid_result, valid_mean, valid_score = check_score(valid_result)
print(valid_mean, valid_score)
submission = pd.read_csv("../input/google-smartphone-decimeter-challenge/sample_submission.csv")
submission = submission[['phone', 'millisSinceGpsEpoch']]
result = test(df_test)
result = result[['phone', 'millisSinceGpsEpoch', 'latDeg', 'lngDeg']]
submission = submission.merge(result, on = ['phone', 'millisSinceGpsEpoch'])
submission.to_csv(f"./models/{'ComplexKalmanFilter1'}/result-{4}result.csv", index = False)<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.160568923875123, 'fraction_numerical': 0.028177833437695678, 'mean_word_length': 2.63104904189672, 'pattern_counts': {'":': 0, '<': 25, '<?xml version=': 0, '>': 29, 'https://': 1, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '6', 'dir': 'jupyter-structured-clean-dedup', 'id': '89172', 'license': 'MIT', 'n_tokens_mistral': 4637, 'n_tokens_neox': 4513, 'n_words': 775, 'path': 'Research/GoogleSmartPhone/code/.ipynb_checkpoints/KF+ANN-checkpoint.ipynb', 'repo_name': 'leeh8911/BeSuperRepo', 'size': '11179'}