首先是建模,设状态变量,
![PIXHAWK 飞控中的EKF姿态估计的欧拉角求解 PIXHAWK 飞控中的EKF姿态估计的欧拉角求解](https://image.shishitao.com:8440/aHR0cHM6Ly93d3cuaXRkYWFuLmNvbS9nby9hSFIwY0hNNkx5OXdhV00wTG5wb2FXMW5MbU52YlM4MU1HUmpNMlJpWWpKa1lUazBZVGczT0RJek1USTNZMlE0WldRek5XTmpOMTlpTG5CdVp3PT0%3D.jpg?w=700&webp=1)
其中w为体系角速度,wa为体系角加速度,ze为重力向量,mu为磁场向量。如果使用惯性矩阵修正当前角加速度,那么:
则机体系角速度为:
![PIXHAWK 飞控中的EKF姿态估计的欧拉角求解 PIXHAWK 飞控中的EKF姿态估计的欧拉角求解](https://image.shishitao.com:8440/aHR0cHM6Ly93d3cuaXRkYWFuLmNvbS9nby9hSFIwY0hNNkx5OXdhV015TG5wb2FXMW5MbU52YlM4MU1UZzRObVUyTXpVMk5URmlPREZoTWpCaFlqY3dNMkl5TXpkaE5qTTJOVjlpTG5CdVp3PT0%3D.jpg?w=700&webp=1)
再根据机体坐标系相对地理坐标系的旋转角速度变换矩阵:
![PIXHAWK 飞控中的EKF姿态估计的欧拉角求解 PIXHAWK 飞控中的EKF姿态估计的欧拉角求解](https://image.shishitao.com:8440/aHR0cHM6Ly93d3cuaXRkYWFuLmNvbS9nby9hSFIwY0hNNkx5OXdhV00wTG5wb2FXMW5MbU52YlM4MFkyWmtaVGd4TkRFMk16STFZMk5qTURNMU5HUXhaVEJsTURnek5qVmlZbDlpTG5CdVp3PT0%3D.jpg?w=700&webp=1)
如果采用指数函数的一阶泰勒近似有:
设
则状态转移A矩阵为:
![PIXHAWK 飞控中的EKF姿态估计的欧拉角求解 PIXHAWK 飞控中的EKF姿态估计的欧拉角求解](https://image.shishitao.com:8440/aHR0cHM6Ly93d3cuaXRkYWFuLmNvbS9nby9hSFIwY0hNNkx5OXdhV015TG5wb2FXMW5MbU52YlM4eFptTTFaV0kyTkRJMk5HRTFZelppT1dOallqVTVZekU0Wm1SbE1HVmpOVjlpTG5CdVp3PT0%3D.jpg?w=700&webp=1)
过程噪声矩阵为:
![PIXHAWK 飞控中的EKF姿态估计的欧拉角求解 PIXHAWK 飞控中的EKF姿态估计的欧拉角求解](https://image.shishitao.com:8440/aHR0cHM6Ly93d3cuaXRkYWFuLmNvbS9nby9hSFIwY0hNNkx5OXdhV014TG5wb2FXMW5MbU52YlM4M04yUTVPV1psTmpSa01URmxPR0UxTVRReE5tUTNaVFUwWW1ZMVl6SmtZMTlpTG5CdVp3PT0%3D.jpg?w=700&webp=1)
分别表示陀螺角速度过程噪声,角加速度过程噪声,加速度过程噪声,磁力计过程噪声。
协方差阵为:
![PIXHAWK 飞控中的EKF姿态估计的欧拉角求解 PIXHAWK 飞控中的EKF姿态估计的欧拉角求解](https://image.shishitao.com:8440/aHR0cHM6Ly93d3cuaXRkYWFuLmNvbS9nby9hSFIwY0hNNkx5OXdhV00wTG5wb2FXMW5MbU52YlM5a1lXSTBOelExTkRreVlUSXhZbVl5WVdaa1kySXhOV015TjJFMk5qWTVabDlpTG5CdVp3PT0%3D.jpg?w=700&webp=1)
测量噪声矩阵:
观测矩阵H为:
![PIXHAWK 飞控中的EKF姿态估计的欧拉角求解 PIXHAWK 飞控中的EKF姿态估计的欧拉角求解](https://image.shishitao.com:8440/aHR0cHM6Ly93d3cuaXRkYWFuLmNvbS9nby9hSFIwY0hNNkx5OXdhV015TG5wb2FXMW5MbU52YlM4NU1XUTNZalZoTmpWbE9XUmtaamN6WkRKalpXSmtNR1l6WmpSa016azRPVjlpTG5CdVp3PT0%3D.jpg?w=700&webp=1)
测量方程为:
![PIXHAWK 飞控中的EKF姿态估计的欧拉角求解 PIXHAWK 飞控中的EKF姿态估计的欧拉角求解](https://image.shishitao.com:8440/aHR0cHM6Ly93d3cuaXRkYWFuLmNvbS9nby9hSFIwY0hNNkx5OXdhV014TG5wb2FXMW5MbU52YlM5aU5tSTBaR1EyWmpjek1qUmxZVFZtWmpnM09HSTNNRFV5WVdJeE1HSmhZMTlpTG5CdVp3PT0%3D.jpg?w=700&webp=1)
引入测量噪声:
EKF的卡尔曼增益矩阵:
状态估计:
![PIXHAWK 飞控中的EKF姿态估计的欧拉角求解 PIXHAWK 飞控中的EKF姿态估计的欧拉角求解](https://image.shishitao.com:8440/aHR0cHM6Ly93d3cuaXRkYWFuLmNvbS9nby9hSFIwY0hNNkx5OXdhV016TG5wb2FXMW5MbU52YlM4eVl6TTBZMlEyTm1KaU5UZ3paV1ZoWTJNMU1qaGhaV1l5WmpnM05EUTBNbDlpTG5CdVp3PT0%3D.jpg?w=700&webp=1)
更新协方差矩阵:
![PIXHAWK 飞控中的EKF姿态估计的欧拉角求解 PIXHAWK 飞控中的EKF姿态估计的欧拉角求解](https://image.shishitao.com:8440/aHR0cHM6Ly93d3cuaXRkYWFuLmNvbS9nby9hSFIwY0hNNkx5OXdhV015TG5wb2FXMW5MbU52YlM5aE16bGpNamMwWVdJd1pqUXhZV1F5T0RZME5EWTFPV1pqWkdOak9EUTJaRjlpTG5CdVp3PT0%3D.jpg?w=700&webp=1)
提取状态信息:
最后得到参考系和机体系的变换矩阵:
转换成欧拉角:
EKF方法计算量大,在计算状态转换阵和基础矩阵时采用了一阶近似,这种近似误差较大,如果计算量允许,可以用2-3阶。
原生固件有关EKF解算的,嵌入式代码是matlab自动生成的,千万不要直接看C代码。。。。