import numpy as np
rng=np.random.default_rng(40)
n=360
t=np.arange(n,dtype=float)  # elke seconde, start08:00 UTC+1
waar=20+0.002*t
z1=waar+rng.normal(0,0.10,n)
z2=waar+rng.normal(0,0.20,n)
z1[120:200]+=2.0  # defect met bias, geen normale ruis
z2[250:281]=np.nan
x=20.0; P=0.25; Q=0.0004
schat=[]; onzeker=[]; afkeur=[0,0]
for k in range(n):
    P+=Q                  # random-walkvoorspelling
    for j,(z,R) in enumerate(((z1[k],0.10**2),(z2[k],0.20**2))):
        if not np.isfinite(z): continue
        innovatie=z-x; S=P+R
        if innovatie**2/S>9:
            afkeur[j]+=1; continue
        K=P/S
        x+=K*innovatie
        P=(1-K)**2*P+K*K*R   # Joseph-vorm, scalar
    schat.append(x); onzeker.append(np.sqrt(P))
schat=np.array(schat)
print("rms sensor1",f"{np.sqrt(np.mean((z1-waar)**2)):.4f}")
print("rms schatting",f"{np.sqrt(np.mean((schat-waar)**2)):.4f}")
print("afgekeurd",afkeur)
print("laatste sigma",f"{onzeker[-1]:.4f}","K")
assert np.sqrt(np.mean((schat-waar)**2))<0.12