import numpy as np
rng=np.random.default_rng(41)
dt=0.1; t=np.arange(0,65,dt)
theta=np.where(t<5,0,10*np.sin((t-5)/12))  # graden voor leesbare uitvoer
omega=np.where(t<5,0,10/12*np.cos((t-5)/12))
gyro=omega+0.20+rng.normal(0,0.05,len(t))
acc=theta+rng.normal(0,0.8,len(t))
acc[(t>=40)&(t<45)]+=8  # translatie: zwaartekrachtmodel tijdelijk fout
bias=gyro[t<5].mean()   # stilstaande ijkfase
alpha=np.exp(-dt/5.0)
g=0.;c=0.;integraal=[];complement=[]
for w,a in zip(gyro,acc):
    g+=dt*(w-bias)
    c=alpha*(c+dt*(w-bias))+(1-alpha)*a
    integraal.append(g);complement.append(c)
print("gyro-bias",round(bias,4),"graden/s")
print("alpha",round(alpha,6))
mask=(t>=5)&~((t>=40)&(t<50))
print("rms complement buiten translatie",
      round(np.sqrt(np.mean((np.array(complement)[mask]-theta[mask])**2)),4),"graden")