# demo/MadgwickExample.R In RAHRS: Data Fusion Filters for Attitude Heading Reference System (AHRS) with Several Variants of the Kalman Filter and the Mahoney and Madgwick Filters

```library(RAHRS)
library(RSpincalc)

# Plot sensor data

par(mfrow=c(3,1)) # 3 graphics horizontaly

legend("topright", c('X','Y','Z'),col=c('red','green','blue'), lty = c(1, 1, 1))

legend("bottomleft", c('X','Y','Z'),col=c('red','green','blue'), lty = c(1, 1, 1))

legend("topright", c('X','Y','Z'),col=c('red','green','blue'), lty = c(1, 1, 1))

#Process sensor data through algorithm
sampleFreq<-256.0
beta<-0.1
q <- c(1, 0, 0, 0)
quatAHRS<-matrix(0,mrows,4)
for(n in 1:(mrows))
{
q <- MadgwickAHRS(1/sampleFreq, beta, q, (pi/180) *cbind(temp\$Gx, temp\$Gy, temp\$Gz), cbind(temp\$Ax, temp\$Ay, temp\$Az), cbind(temp\$Mx, temp\$My, temp\$Mz))
quatAHRS[n,]<-q
}

Euler <- -(180/pi) * Q2EA(Qconj(quatAHRS),'xyz')

# Plot algorithm output as Euler angles
dev.new()
par(mfrow=c(1,1))
plot(MadgwickData[,1],Euler[,1],col='red',type='l',main='Euler angles', xlab="Time (s)", ylab="Angle (deg)",ylim=c(-200,200));par( new=TRUE)