sfrmat3 C++实现

实现功能，与matlab比对结果一致，待整理。