17维的平方根容积卡尔曼滤波程序如何优化？

我用C写了一个17维的平方根容积卡尔曼滤波程序，结果迭代一次速度为0.26s，加了-o3为0.052s，但现在还不满足要求，算法中基本都是for循环。想请问一下，该如何优化程序，怎么充分发挥DSP的硬件优势？要不要配置定点/浮点，还有为什么没有乘法的浮点库？