##############################################################################################
#Created by: Stefan Metzger (stefan.met@gmail.com)
#Last change by: Stefan Metzger (2013-12-29)
##############################################################################################


Mbg <- function(roll, pich, hdng, XYZ) {
#allocate ouput matrix North / East / Down
NED=matrix(nrow=length(hdng), ncol=3)

#carry out rotation
NED[,1]=
    cos(hdng)*cos(pich)*XYZ[,1] +
    (-sin(hdng)*cos(roll)+cos(hdng)*sin(pich)*sin(roll))*XYZ[,2] +
    (sin(hdng)*sin(roll)+cos(hdng)*sin(pich)*cos(roll))*XYZ[,3]
NED[,2]=
    (sin(hdng)*cos(pich))*XYZ[,1] + 
    (cos(hdng)*cos(roll)+sin(hdng)*sin(roll)*sin(pich))*XYZ[,2] +
    (sin(hdng)*sin(pich)*cos(roll)-cos(hdng)*sin(roll))*XYZ[,3]
NED[,3]=
    (-sin(pich))*XYZ[,1] +
    (cos(pich)*sin(roll))*XYZ[,2] +
    (cos(pich)*cos(roll))*XYZ[,3]

#return the result
veloTA2GC<-list(list(NED[,1], NED[,2], NED[,3]))
  attributes(veloTA2GC)$names<-"veloTA2GC"
  attributes(veloTA2GC$veloTA2GC)$names<-c("x","y","z")
return(veloTA2GC)
}