demo/model.ahrs.UKF.QUATERNION.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)

data(anglesGyroLib)

accl.coefs <- matrix(c(-0.0771, -0.0817, -0.0769,  0.0066, -0.0032,  0.0001,  0.0090, -0.0023, -0.0035, -0.0281,  0.0430,  0.0306),ncol=1)
magn.coefs <- matrix(c(-0.3801, -0.3108, -0.3351,  0.0342,  0.0109, -0.0066,  0.0070, -0.0696, -0.0492,  0.0552,  0.0565,  0.0190),ncol=1)
gyro.coefs <- vector(mode='numeric',12)

# Number of simulation steps
Nsim <- dim(anglesGyroLib)[1]
m <- matrix(unlist(anglesGyroLib[,c('Mx','My','Mz')]),ncol=3,nrow=Nsim,byrow=FALSE)/3000
a <- matrix(unlist(anglesGyroLib[,c('Ax','Ay','Az')]),ncol=3,nrow=Nsim,byrow=FALSE)/3000
w <- matrix(unlist(anglesGyroLib[,c('Wx','Wy','Wz')]),ncol=3,nrow=Nsim,byrow=FALSE)/3000

# Define Output data Arrays
R.out <- matrix(0,ncol=3,nrow=Nsim)
dw.out <- matrix(0,ncol=3,nrow=Nsim)
w.out <- matrix(0,ncol=3,nrow=Nsim)
a.out <- matrix(0,ncol=3,nrow=Nsim)
m.out <- matrix(0,ncol=3,nrow=Nsim)
TRIAD.out <- matrix(0,ncol=3,nrow=Nsim)

## Calibrate magnetometers
B <- matrix(as.numeric(c(magn.coefs[1], magn.coefs[4], magn.coefs[5],
                         magn.coefs[6], magn.coefs[2], magn.coefs[7],
                         magn.coefs[8], magn.coefs[9], magn.coefs[3])),nrow=3,ncol=3,byrow=TRUE)
B0 <- matrix(as.numeric(c(magn.coefs[10],magn.coefs[11],magn.coefs[12])),nrow=3,ncol=1)

for (n in 1:Nsim) m.out[n,] <- t((diag(3) - B) %*% (matrix(m[n,],3,1) - B0))

## Calibrate accelerometers
B <- matrix(as.numeric(c(accl.coefs[1], accl.coefs[4], accl.coefs[5],
                         accl.coefs[6], accl.coefs[2], accl.coefs[7],
                         accl.coefs[8], accl.coefs[9], accl.coefs[3])),nrow=3,ncol=3,byrow=TRUE)
B0 <- matrix(as.numeric(c(accl.coefs[10],accl.coefs[11],accl.coefs[12])),nrow=3,ncol=1)

for (n in 1:Nsim) a.out[n,] <- t((diag(3)-B) %*% (matrix(a[n,],3,1) - B0))

## Calibrate gyroscopes
B <- matrix(as.numeric(c(gyro.coefs[1], gyro.coefs[4], gyro.coefs[5],
                         gyro.coefs[6], gyro.coefs[2], gyro.coefs[7],
                         gyro.coefs[8], gyro.coefs[9], gyro.coefs[3])),nrow=3,ncol=3,byrow=TRUE)
B0 <- matrix(as.numeric(c(gyro.coefs[10],gyro.coefs[11],gyro.coefs[12])),nrow=3,ncol=1)

for (n in 1:Nsim) w.out[n,] <- t((diag(3)-B) %*% (matrix(w[n,],3,1) - B0))

## AHRS Parameters
#Magnetic Field Vector In Navigation Frame
Parameters<-list(mn=0,an=0,dT=0)
Parameters$mn = matrix(c(0.315777529635464, 0.057133095826051, -0.947111588535720),ncol=1)
#Acceleration vector In Navigation Frame
Parameters$an = matrix(c(0, 0, -1),ncol=1)
#Sampling Rate 1/Hz
Parameters$dT  = c(1/100)

#Initial attitude quaternion value
q = EA2Q( c(0.0,0.0,0.0),'zyx' ) 
#q = single(q)
#Initial State Vector
Filter<-list(x=0,P=0,R=0,Q=0)
Filter$x = matrix(c(t(q), 0, 0, 0),ncol=1)
#Initial Covariance Matrix
Filter$P = diag(1e-5,7)
#Measuremens Noise
Filter$R = diag(1e0,6)
#Syatem Noise
Filter$Q = diag(c(1e-4, 1e-4, 1e-4, 1e-4, 1e-10, 1e-10, 1e-10))
Sensors<-list(w=0,a=0,m=0)
## Main loop
for (n in 1:Nsim)
    {
    Sensors$w = (matrix(w.out[n,],ncol=1))
    Sensors$a = (matrix(a.out[n,],ncol=1))
    Sensors$m = (matrix(m.out[n,],ncol=1))

    ## AHRS Part
    Filter = ahrs.UKF.QUATERNION(Filter, Sensors, Parameters)
    #[R.out(i,1) R.out(i,2) R.out(i,3)] = ...
  R.out[n,] <- Q2EA(Filter$x[1:4])
    dw.out[n,] =  c(Filter$x[5], Filter$x[6], Filter$x[7])

    ## TRIAD Part
    W1 = matrix(a.out[n,],nrow=1) / norm(matrix(a.out[n,],nrow=1),'f') 
    W2 = matrix(m.out[n,],nrow=1)/norm(matrix(m.out[n,],nrow=1),'f')
    
    V1 = (matrix(Parameters$an,nrow=1))/norm(matrix(Parameters$an,nrow=1),'f')#t
    V2 = (matrix(Parameters$mn,nrow=1))/norm(matrix(Parameters$mn,nrow=1),'f')#t
    
    Ou1 = W1
    Ou2 = cross(W1,W2)/norm(cross(W1,W2),'f')
    Ou3 = cross((W1),cross(W1,W2))/norm(cross(W1,W2),'f')

    R1 = V1
    R2 = (cross(V1,V2)/norm(cross(V1,V2),'f'))#t
    R3 = (cross((V1),cross(V1,V2))/norm(cross(V1,V2),'f'))#t
    
    Mou = cbind(t(Ou1), t(Ou2), t(Ou3))
    Mr = cbind(t(R1), t(R2), t(R3))
    
    A = Mou %*% t(Mr)

    # Calculate angles
   # [TRIAD.out(i,1) TRIAD.out(i,2) TRIAD.out(i,3)] = DCM2EA.outeml(A)
    TRIAD.out[n,] <- DCM2EA(A)#rotMat2euler(A)
   
}

#pdf('EKF.QUATERNION.pdf')
## Plot Results
#psi - Angle around Z axis
#theta - Angle around Y axis
#gamma - Angle around X axis

plot(TRIAD.out[,1],col='red',ylim=c(-4,4), type='l',main='UKF quaternion TRIAD / AHRS',xlab='Time 10 msec (100 Hz)', ylab='Euler angles')
par(new=T)
plot(TRIAD.out[,2],col='blue',ylim=c(-4,4), type='l',main='',xlab='', ylab='')
par(new=T)
plot(TRIAD.out[,3],col='green',ylim=c(-4,4), type='l',main='',xlab='', ylab='')
par(new=T)
plot(R.out[,1],col='orange',ylim=c(-4,4), type='l',main='',xlab='', ylab='')
par(new=T)
plot(R.out[,2],col='cyan',ylim=c(-4,4), type='l',main='',xlab='', ylab='')
par(new=T)
plot(R.out[,3],col='magenta',ylim=c(-4,4), type='l',main='',xlab='', ylab='')
abline(h=(seq(-4,4,1)), col="lightgray", lty="dotted")
abline(v=(seq(0,16000,1000)), col="lightgray", lty="dotted")
legend("topleft", c( expression(paste(psi,plain(TRIAD))) ,expression(paste(theta,plain(TRIAD))),
expression(paste(gamma,plain(TRIAD))),expression(paste(psi,plain(AHRS))) ,expression(paste(theta,plain(AHRS))),
expression(paste(gamma,plain(AHRS)))),col=c('red','blue','green','orange','cyan','magenta'), lty = c(1, 1, 1, 1, 1, 1),bg='white')


dev.new()
yl<-c(-6*1e-4,4*1e-4)
plot(dw.out[,1],col='blue',ylim=yl, type='l',main='UKF quaternion estimated gyroscope bias drift',xlab='Time 10 ms (100 Hz)',ylab='Estimated bias radians/sec')
par(new=T)
plot(dw.out[,2],col='green',ylim=yl, type='l',main='',xlab='', ylab='')
par(new=T)
plot(dw.out[,3],col='red',ylim=yl, type='l',main='',xlab='', ylab='')
abline(h=(seq(-4*1e-4,4*1e-4,1*1e-4)), col="lightgray", lty="dotted")
abline(v=(seq(0,16000,1000)), col="lightgray", lty="dotted")
legend("topleft", c( expression(paste(omega,plain(X0))) ,expression(paste(omega,plain(Y0))),
expression(paste(omega,plain(Z0)))),col=c('red','blue','green','orange','cyan','magenta'), lty = c(1, 1, 1))

#dev.off()