FIRfilter.py

import sys
import pandas as pd
import matplotlib as mpl
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
import matplotlib.pyplot as plt

filename = sys.argv[1]
df = pd.read_csv(filename)
dt = 0.05
i = np.arange(0,len(df)*dt,dt)
x = df['x'].values
y = df['y'].values
z = df['z'].values
d = df['d'].values
t = df['t'].values
p = df['p'].values
x2 = df['x'].rolling(window=2).mean()
y2 = df['y'].rolling(window=2).mean()
z2 = df['z'].rolling(window=2).mean()
x3 = df['x'].rolling(window=3).mean()
y3 = df['y'].rolling(window=3).mean()
z3 = df['z'].rolling(window=3).mean()
x4 = df['x'].rolling(window=4).mean()
y4 = df['y'].rolling(window=4).mean()
z4 = df['z'].rolling(window=4).mean()
x5 = df['x'].rolling(window=5).mean()
y5 = df['y'].rolling(window=5).mean()
z5 = df['z'].rolling(window=5).mean()
d2 = df['d'].rolling(window=2).mean()
t2 = df['t'].rolling(window=2).mean()
p2 = df['p'].rolling(window=2).mean()
d3 = df['d'].rolling(window=3).mean()
t3 = df['t'].rolling(window=3).mean()
p3 = df['p'].rolling(window=3).mean()
d4 = df['d'].rolling(window=4).mean()
t4 = df['t'].rolling(window=4).mean()
p4 = df['p'].rolling(window=4).mean()
d5 = df['d'].rolling(window=5).mean()
t5 = df['t'].rolling(window=5).mean()
p5 = df['p'].rolling(window=5).mean()
df = pd.concat([df,x2,y2,z2,x3,y3,z3,x4,y4,z4,x5,y5,z5,d2,t2,p2,d3,t3,p3,d4,t4,p4,d5,t5,p5], axis=1, join='inner')
df.to_csv("aaa.csv")
print(df)

plt.subplot(6,1,1)
#plt.plot(i, x, '-', lw=1)
#plt.plot(i, x2, '-', lw=1)
#plt.plot(i, x3, '-', lw=1)
#plt.plot(i, x4, '-', lw=1)
plt.plot(i, x5, '-', lw=1)
plt.xlabel('Time')
plt.ylabel('x')
#plt.title('x')
plt.grid(True)
plt.subplot(6,1,2)
plt.plot(i, y, '-', lw=1)
#plt.plot(i, y2, '-', lw=1)
#plt.plot(i, y3, '-', lw=1)
#plt.plot(i, y4, '-', lw=1)
plt.plot(i, y5, '-', lw=1)
plt.xlabel('Time')
plt.ylabel('y')
#plt.title('y')
plt.grid(True)
plt.subplot(6,1,3)
#plt.plot(i, z, '-', lw=1)
#plt.plot(i, z2, '-', lw=1)
#plt.plot(i, z3, '-', lw=1)
#plt.plot(i, z4, '-', lw=1)
plt.plot(i, z5, '-', lw=1)
plt.xlabel('Time')
plt.ylabel('z')
#plt.title('z')
plt.grid(True)
plt.subplot(6,1,4)
#plt.plot(i, d, '-', lw=1)
#plt.plot(i, d2, '-', lw=1)
#plt.plot(i, d3, '-', lw=1)
#plt.plot(i, d4, '-', lw=1)
plt.plot(i, d5, '-', lw=1)
plt.xlabel('Time')
plt.ylabel('Distance')
#plt.title('Distance')
plt.grid(True)
plt.subplot(6,1,4)
#plt.plot(i, d, '-', lw=1)
#plt.plot(i, d2, '-', lw=1)
#plt.plot(i, d3, '-', lw=1)
#plt.plot(i, d4, '-', lw=1)
plt.plot(i, d5, '-', lw=1)
plt.xlabel('Time')
plt.ylabel('Distance')
#plt.title('Distance')
plt.grid(True)
plt.subplot(6,1,5)
plt.plot(i, t, '-', lw=1)
plt.plot(i, t2, '-', lw=1)
plt.plot(i, t3, '-', lw=1)
plt.plot(i, t4, '-', lw=1)
plt.xlabel('Time')
plt.ylabel('Theta')
#plt.title('Theta')
plt.grid(True)
plt.subplot(6,1,6)
plt.plot(i, p, '-', lw=1)
plt.plot(i, p2, '-', lw=1)
plt.plot(i, p3, '-', lw=1)
plt.plot(i, p4, '-', lw=1)
plt.xlabel('Time')
plt.ylabel('Phi')
#plt.title('Phi')
plt.grid(True)
#plt.savefig('')
plt.show()