I’m trying to implement a Kalman filter on a noisy sine signal, but I’m getting a phase lag on the filtered output which is also higher in amplitude.

The noisy signal code is taken per Crawl Cycle’s answer. The parameters I have for the Kalman filter are the numbers for a real feedback loop that I’m trying to simulate. I tried lowering the control law but that didn’t have any effect. Here is my code:

import numpy as np
from matplotlib.ticker import FuncFormatter, MultipleLocator
import matplotlib.pyplot as plt
%matplotlib notebook

# noisy signal
x = np.linspace(0, 10 * np.pi, num=600)
n = np.random.normal(scale=8, size=x.size)
s = 100 * np.sin(x)
y = 100 * np.sin(x) + n

#kalman filter
x_estimate1 = np.array([]) #filtered values
    
x_init = 9.86018035  # initial guess for x_0|0
x0 = 9.86618035 # initial guess for the current state 0
T_s = np.float32(1e-5)  # sampling time
tau = np.float32(1e-3) # lock-in rolloff time (1/f)
omega = (2*np.pi)/tau # single pole for the LA transfer function 
phi = 1+omega*T_s 
d_var = 0 # process noise
s_var = 7.22920854 # measurement noise
k_omega = 1 # control law

counter = 1
for signal in y:
    if counter == 1:
        x_next = phi*x_init + (-omega*T_s)*k_omega*signal # predict the next state x_n
        e_pre_var = (x0 - x_init)*(x0 - x_init) 
        x_estimate1 = np.append(x_estimate1, x_next)
        counter += 1

    else:
        u = -omega*T_s*k_omega*signal;
        K_next = (phi*phi*e_pre_var + d_var)/(phi*phi*e_pre_var + d_var + s_var)
        x_curr = (1-K_next)*x_next + K_next*signal # estimate the current state
        x_next = phi*x_curr + (-omega*T_s*u) # predict the next state
        e_next_var = (1-K_next)*(1-K_next)*(phi*phi*e_pre_var + d_var) + K_next*K_next*s_var

        # make the next values the old values for next cycle
        K_pre = K_next
        e_pre_var = e_next_var


        # Assign the calculated value to the output signal
        x_estimate1 = np.append(x_estimate1, x_next)
        
        counter += 1

#plot

plt.scatter(x, x_estimate1, s=10, label='estimate1')
plt.scatter(x, y, s=10, label='Total')
plt.scatter(x, s, s=10, label='sine')

plt.legend()
plt.show()

Here is the output I get