AP_Math: move zeroing to header, use memset, reuse in identity

[peterbarker]

this method is in ITCM memory on STM32 - which makes small optimisations worthwhile

Board                    AP_Periph  blimp  bootloader  copter  heli  iofirmware  plane  rover  sub
CubeOrange-periph-heavy  *                                                                     
CubeRedPrimary                      -72    *           -72     -64               -72    -72    -64
Durandal                            -64    *           -64     -64               -64    -64    -64
Hitec-Airspeed           *                                                                     
KakuteH7-bdshot                     -64    *           -64     -64               -64    -64    -64
MatekF405                           -48    *           -40     -48               -40    -32    -40
Pixhawk1-1M-bdshot                  -40                -40     -48               -40    -40    -48
f103-QiotekPeriph        *                                                                     
f303-Universal           *                                                                     
iomcu                                                                *                         
revo-mini                           -40    *           -40     -40               -48    -40    -48
skyviper-v2450                                         -48                                     

[peterbarker]

I assumed it was the memset - but didn't check.