python walk

import matplotlib.pyplot as plt
import numpy as np
import math
a = np.loadtxt('/Users/matt/Downloads/output.txt')
n_iter = a[:,0].size-1
walk=np.ones(n_iter)
step_count=np.ones(n_iter)
angle_y_gyro_sum = np.zeros(n_iter)
def kalman_filter(z):
    Q = 1e-5 # process variance
    xhat=np.zeros(n_iter)      # a posteri estimate of x
    xhat[0] = z[0]
    P= 1
    R= 0.01
    for k in range(1,n_iter):
        xhatminus = xhat[k-1]
        Pminus = P+Q
        K = Pminus/( Pminus +R )
        xhat[k] = xhatminus+ K*(z[k]- xhatminus)
        P = (1-K)*Pminus        
    return xhat

# Kalman filer of IMU
accX = kalman_filter(a[:,0]) 
accY = kalman_filter(a[:,1]) 
accZ = kalman_filter(a[:,2]) 
gyroX = kalman_filter(a[:,3])
gyroY = kalman_filter(a[:,4])
gyroZ = kalman_filter(a[:,5])

#得到rms
for k in range(0,n_iter):
    angle_y_gyro_sum[k] = angle_y_gyro_sum[k-1] - gyroY[k] 
    walk[k] = math.sqrt(accX[k]*accX[k] + accY[k]*accY[k]+ accZ[k]* accZ[k])

# 得到平均值  60Hz Sample --> 1s
acc_average = np.mean(walk)
max_thrshold = acc_average + 0.005
last_step = 0
print(acc_average)
print(max_thrshold)

if walk[1] > walk[0] : 
    moving_up_flag = True
else:
    moving_up_flag = False

for k in range(1,n_iter):
   
    if moving_up_flag == True:             #up
        if walk[k] < walk[k-1]:     #max point 
            moving_up_flag = False 
            if walk[k-1] > max_thrshold :  #最大值要大于max_thrshold
                step_count[k] = 1.1   
                if (k - last_step)< 30 :
                    step_count[k] = 1.0                  
                last_step = k
    else:
        if walk[k] > walk[k-1]:     #max point 
            moving_up_flag = True
            

plt.rcParams['figure.figsize'] = (15, 4)

plt.figure()
plt.plot(walk,'r',label='walk')
plt.plot(step_count,'g',label='step_count')

# plt.plot(angle_y_gyro_sum/90,'b',label='angle_y_gyro_sum')
plt.legend()
plt.show()