px4飞控磁罗盘校准算法理论与代码结合解读

【实例简介】
px4飞控磁罗盘校准算法理论与代码结合解读，有分析图解，有代码注释，有理论有实践
设 (((() 假设按时间顺序,依次的到了组采样点:( 当得到第一组采样点( 步骤一:根据式()计算(,,)、根据()式计算 ++ 十++ 在此需要对式()进行说明的是 和(,,)注意并不是 在每次取得新采样点进行计算时进行更新,在每次取得新釆样点后进行计算吋 和 )保持不变。因此,(,,)右下脚标数字只与 ,)右下脚标数字保持一致。在木次计算(即第一次得到采样点) 时 和( )的值为设定的初值如公式(),代码如图、图 213 vold Compasscalibrator::reset state(i 214 samples collected = 0: 215 samples thinned 0 216 params radius 200, 217 params ofset zero)i 218 params.niag=Vr3f(1.0千,1.0千,1.0f) 219 params. of-diag zero()i 220 221 ini七ia] ize fit(); 222L 图 和( ,)的值为设定 80 void Vector:: zero (vold) 81日 82 x=0.0f; 83 Y=0.0f; 84 z=0.0f 85-} 图图中代码调用函数 步骤二:求 的矩阵: () 步骤三:计算 匚EL 步骤四:计算[]、[ 在计算[]时,计算公式如下 上一计算值总和 本次计算值 这里对式()需要进行说明的是:对的计算是把以前计算结果得总 和加上本次计算结果,在第一次对数据进行计算时,不存在之前的计算。在这里, 以前计算结果定义为对该量赋与的初值。在本次计算中,式()可写成如下: [[]=[[][[]b[[] 同理: 初值 初值 初值 在计算[·]时用的计算式子与式()一致,且在第一次做计算 时同样也赋于了值为的初值: 当得到第二组采样点 )后,同样执行在得到第一组采样 点里的个步骤,这样可以得到 十十 十 umO []=[[]+[[]=∑[[ []=[[][[]=[[]-[[=∑[[ ]=∑[·] ]=∑[ ]=∑[ 当得到第三组采样点后: 当得到第组采样点(, )后: ++++ ]=[[ [,=∑[[] []=[[]l=([[=∑[[ [[]=[]-[[]=∑[[ ]=∑[ ]-∑ ]=∑[ 到这里,代码中的和 [](,,),已分别更新为 式()、( 以上内容为 所共有的准备内容;以下部分为算法所特有的: 将步长系数λ(初值为)和衰减因子β(初值为)按照式()、() 融入到中,设: ∑[[]+λ∑[[]∑[]∑[[ ∑[[]∑[[]+2∑L[]∑L[ ∑[[]∑[[]∑[]+∑[[] ∑[[]∑[[]∑[[] +1 ∑[[+∑[[∑[[]∑[[ [tΣ[[]+aΣ[[∑[[ ∑[∑[[]∑[[]+∑[[ ∑[[]∑[]∑[[] 分别求式()、()的逆矩阵,记为: 设 +表示为如()、()式 至此,代码里的 和 已更新如()、()式; ()式。利用式()、()、()以及相关参数的初值进行第一次迭代(拟 由算法,迭代方程为: 在式()中:为拟合的参数,在代码里对应的是 (即函数 的个状态量: )在第一次迭代中为各状 态量的初值,如图、图所示与式()一致;与上文中步长系数一 致;为×单位矩阼;与式()一致;与式()一致。 迭代过程如下(第一次) 计算 ∑[ ]+∑[ ∑[ ]+∑[·]+∑[ 计算 初值) 对()式同样做上述迭代,可得: 初值 经过以上计算设利川「+更新后的个参数属于 这一类;利 用[+1更新后的个参数属于 这一类。 对组采样数据,计算基于 的残差的均方值: 【实例截图】
【核心代码】