问题：carsim中的汽车初始速度设置为0汽车也会以微小的速度运动
 
**原理：**自动挡的车不踩油门也是会走的,因为自动挡车没有离合踏板,当你的档位进入行车档位的时候,实际上离合就已经处于连通状况,这个时候没有刹车的阻力自然汽车缓缓前行
 
所以即使carsim中设置车辆的初始速度为0，车辆也会运动，除非施加一定的制动压力，车才会不动。

 
 
ApplyForce、ApplyImpulse和SetLinearVelocity。
 
它们都是b2Body类的公共方法，而且它们都接收一个b2Vec2类型向量参数。关于向量的知识。
 1.力，循序渐进——ApplyForce
 
顾名思义，ApplyForce方法会在刚体上施加一个力。学过物理力学的同学都知道，F=ma，有了力F就有了加速度a，有了加速度，物体就会有速度，就会慢慢动起来。(但是不会立马动起来，因为力不度)。
 
ApplyForce施加的力可以叠加，如
 
ballBody->ApplyForce(force1,ballBody->GetWorldCenter());//GetWorldCenter()方法用来获取刚体的重心
 
ballBody->ApplyForce(force2,ballBody->GetWorldCenter());
 
这样施加给ballBody的力就为force1与force2的和。
 
举个简单的例子，小明推一个静止的箱子，箱子不会立马飞出去，而是慢慢的、越来越快的动起来(减速也一样)。
 
2.速度，叠加——ApplyImpulse
 
与ApplyForce不同，ApplyImpulse不会产生力，而是直接影响刚体的速度。通过ApplyImpulse方法添加的速度会与刚体原有的速度叠加，产生新的速度。
 
3.一触即发——SetLinearVelocity
 
setLinearVelocity与ApplyImpulse一样，直接影响刚体的速度。不一样的是，setLinearVelocity添加的速度会覆盖刚体原有的速度。不过，在SetLinearVelocity方法不会自动唤醒sleeping的刚体，所以在调用该方法之前，记得将刚体body.wakeUp()一下。
 
b2Vec2 velocity = b2Vec2(x,y);
  ballBody->SetLinearVelocity(velocity);//设置刚体的速度
 
velocity1 = ballBody->GetLinearVelocity();//得到刚体的速度
 

 

磁力计和加速度计初始姿态解算
 
四旋翼的初始姿态是通过磁力计和加速度计来解算的，缺一不可。学习过程中遇到了不少问题，想通了总结如下：
 
初始姿态解算原理
牛顿迭代法
迭代初值选取
万向节锁
C++代码示例
 
初始姿态解算原理
 
导航坐标系选为东北天，则重力加速度和磁力计在导航坐标系中分别表示为$[0,0,-g]^T$，$[0,m_y^n,m_z^n]^T$。假设机身坐标系下，加速度计和磁力计输出分别为$[a_x,a_y,a_z]^T$，$[m_x^b,m_y^b,m_z^b]^T$。若飞机加速度为0，且加速度计安装位置在飞机重心（原因打算后面的博客再写），则： 
 
 $$\begin{equation}
\left[
\begin{array}{ccc}
a_x\\a_y\\a_z
\end{array}
\right]=
R_n^b
\left[
\begin{array}{ccc}
0\\0\\-g
\end{array}
\right](1)
\end{equation}$$  

 

 $$\begin{equation}
\left[
\begin{array}{ccc}
0\\m_y^n\\m_z^n
\end{array}
\right]=
R_b^n
\left[
\begin{array}{ccc}
m_x\\m_y\\m_z
\end{array}
\right](2)
\end{equation}$$  

 其中，

$R_b^n=(R_n^b)^T$。

$R_n^b$可以用四元数表示形式为： 

 

 $$\begin{equation}
\left[
\begin{array}{ccc}
q_0^2+q_1^2-q_2^2-q_3^2&&2(q_1q_2+q_0q_3)&&2(q_1q_3-q_0q_2)\\
2(q_1q_2-q_0q_3)&&q_0^2-q_1^2+q_2^2-q_3^2&&2(q_2q_3+q_0q_1)\\
2(q_1q_3+q_0q_2)&&2(q_2q_3-q_0q_1)&&q_0^2-q_1^2-q_2^2+q_3^2
\end{array}
\right]
\end{equation}$$  

 这里的表示形式和上一篇博客中的不一样，是为了和

$Matlab$保持一致。

$R_n^b$的欧拉角形式可参见上一篇博客。 

 选其中四个方程如下： 

 

 $$\begin{eqnarray}
\left\{
\begin{array}{ccc}
2(q_1q_3-q_0q_2)+\frac{a_x}{g}=0\\
2(q_2q_3+q_0q_1)+\frac{a_y}{g}=0\\
q_0^2-q_1^2-q_2^2+q_3^2+\frac{a_z}{g}=0\\
m_x^b(q_0^2+q_1^2-q_2^2-q_3^2)+m_y^b(2(q_1q_2-q_0q_3))+m_z^b(2(q_1q_3+q_0q_2))=0\\
\end{array}
\right.
\end{eqnarray}$$ 
 
 
牛顿迭代法
 
将$n$元非线性方程组写成如下形式： 

 $$\begin{eqnarray}
   \left\{
   \begin{array}{ccc}
   f_1(x_1,x_2,...,x_n)=0\\
   f_2(x_1,x_2,...,x_n)=0\\
   \vdots\\
   f_n(x_1,x_2,...,x_n)=0\\
   \end{array}
   \right.
\end{eqnarray}$$  
 简写为：$F(x)=0$， 
 其中：$x=(x_1,x_2,\ldots,x_n)^T,F(x)=(f_1(x),f_2(x),\ldots,f_n(x))^T$ 
 若方程组存在解$x^*\in int(D)$，在$x^*$的某个开邻域$D_0=\{x\mid \|x-x^*\|<\delta,\delta>0\}\subset D$内$F(x)$可微，设$x^{(k)}\in D_0$是方程组的第$K$个近似解。由一阶泰勒公式可得： 

$f_i(x)\approx f_i(x^{(k)})+\mathop{\sum_{j=1}^{n}}\frac{\partial f_i(x^{(k)})}{\partial x_j}(x_j-x_j^{(k)});(i=1,2,...,n)$ 
 可以近似写为： 
 $f_i(x^{(k)})+\mathop{\sum_{j=1}^{n}}\frac{\partial f_i(x^{(k)})}{\partial x_j}(x_j-x_j^{(k)})=0;(i=1,2,...,n)$ 
 写成矩阵形式： 
 $F'(x^{(k)})(x-x^{(k)})=-F(x^{(k)})$ 
 则迭代公式为： 
 $x^{(k+1)}=x^{(k)}-F'(x^{(k)})^{-1}F(x^{(k)});(k=0,1,2...)$ 
 算法设计：
 
在$x^*$附近选取$x^{(0)}\in D$，给定精度水平$\epsilon >0$和最大迭代次数$M$。
对于$k=0,1,...M$执行 
 
  
计算$F(x^{(k)})$和$F'(x^{(k)})$
求解关于$\Delta x^{(k)}$的线性方程组：$F'(x^{(k)})\Delta x^{(k)}=-F(x^{(k)})$
若$\|\Delta x^{(k)}\|/\|x^{(k)}\|\leq\epsilon$，取$x^*\approx x^{(k)}$，并停止计算，否则继续向下执行
计算$x^{(k+1)}=x^{(k)}+\Delta x^{(k)}$
若$k<M$，继续迭代，否则停止计算。
 
 
迭代初值选取
 
迭代初值的选取会影响迭代次数和迭代结果。首先，初值离真值越近，迭代次数越少。此外，如果方程有多个解，选取不同的初值会得到不同的解，而正确的解只有一个，这就需要我们选取的初值必须距离正确的解最近。 
 上面我们选取了四个方程，理论上有很多个解。最开始时，我将初值固定为$[1,0,0,0]$，某些情况下解算出来的偏航角会差180°。这意味着，最终的结果满足选取的四个方程，却不满足剩下的两个。那为什么俯仰角和滚转角解算不会出错呢？这个也是有原因的： 
 用欧拉角表示$R_n^b$，并将(1)式展开可以得到： 
 
 $$\begin{eqnarray}
\left\{
\begin{array}{ccc}
sin(\theta)=\frac{a_x}{g}\\
cos(\theta)sin(\phi)=\frac{-a_y}{g}\\
cos(\theta)cos(\phi)=\frac{-a_z}{g}
\end{array}
\right.
\end{eqnarray}$$  

 可以发现，当

$cos(\theta)\neq0$时，仅根据加速度计的输出便可以得到俯仰角和滚转角： 

 

 $$\begin{eqnarray}
\left\{
\begin{array}{ccc}
\theta=asin(a_x/g)\\
\phi=atan(-a_y,-a_z)\\
\end{array}
\right.(3)
\end{eqnarray}$$  

 我们选取的四个方程包括加速度计的三个方程，迭代结果一定满足(1)式，而(1)式又唯一确定俯仰角和滚转角，因此二者解算结果永远不会出错。 

 那问题来了，我们该怎样选取合适的初值呢？这个也不难！如下： 

 

 $$\begin{equation}
\left[
\begin{array}{ccc}
a_x&&m_x^b\\
a_y&&m_y^b\\
a_z&&m_z^b\\
\end{array}
\right]=
\left[
\begin{array}{ccc}
R_{12}&&R_{13}\\
R_{22}&&R_{23}\\
R_{32}&&R_{33}\\
\end{array}
\right]
\left[
\begin{array}{ccc}
0&&m_y^n\\
-g&&m_z^n\\
\end{array}
\right](4)
\end{equation}$$ 
 
$R_{ij}$是旋转矩阵的后两列，等式右边的2x2方阵是可逆的，并且是个常数矩阵，这样也就可以求出这两列元素。俯仰角和滚转角都已知的情况下，根据旋转矩阵第二列便可以求出偏航角的正弦和余弦，进而得到偏航角。把得到的欧拉角转化为四元数来作为迭代的初值，就一定可以得到正确的解。
 
 
万向节锁
 
笔者解决了初值选取的问题之后，发现在俯仰角为$\pm90^\circ$的时候，不仅迭代次数剧增，而且迭代结果偏航角和滚转角都会出错。这又是为什么呢？想了又想，想了又想，发现了传说中的万向节锁！ 
 我在上面特意提到：当$cos(\theta)\neq 0$时，根据加速度计可以正确得到俯仰角和滚转角，是因为如果$cos(\theta)=0$，$\phi=atan(0,0)$是无意义的。况且实际中加速度计都存在误差，如果俯仰角过分接近$\pm90^\circ$，加速度计$a_y$和$a_z$轴的理论值会被误差淹没，也就无法得到正确的滚转角和偏航角。目前，笔者只有一个解决办法，对初始姿态解算时的俯仰角进行限制，笔者设置在$\pm75^\circ$之间。
 
C++代码示例
 
int NewtonIteration(cVector<3> &acc, cVector<3> &mag, cQuaternion &quat)
{
    int maxStep = 10;
    int curStep = 0;
    double error = 1E-8;
    cMatrix<4, 4> dF;
    cVector<4>  F;
    cQuaternion dquat;
    cEulerAngle angle;
    if (fabs(acc(3))<9.5)
    {
        double sin_yaw, cos_yaw, sin_pitch, cos_pitch, sin_roll, cos_roll;
        sin_pitch = acc(1) / g;
        cos_pitch = sqrt(1 - sin_pitch*sin_pitch);
        sin_roll = -acc(2) / (g*cos_pitch);
        cos_roll = -acc(3) / (g*cos_pitch);
        sin_yaw = (mag(1)*g + acc(1)*mnz) / (g*mny*cos_pitch);
        cos_yaw = ((mag(2)*g + acc(2)*mnz) / (g*mny) - sin_roll*sin_pitch*sin_yaw) / cos_roll;
        angle(1) = atan2(sin_roll, cos_roll);
        angle(2) = asin(sin_pitch);
        angle(3) = atan2(sin_yaw, cos_yaw);
        angle.toQuat(quat);
        do{
            curStep++;
            dF(1, 1) = -quat(3);
            dF(1, 2) = quat(4);
            dF(1, 3) = -quat(1);
            dF(1, 4) = quat(2);
            dF(2, 1) = quat(2);
            dF(2, 2) = quat(1);
            dF(2, 3) = quat(4);
            dF(2, 4) = quat(3);
            dF(3, 1) = 2 * quat(1);
            dF(3, 2) = -2 * quat(2);
            dF(3, 3) = -2 * quat(3);
            dF(3, 4) = 2 * quat(4);
            dF(4, 1) = 2 * (mag(1) * quat(1) - mag(2) * quat(4) + mag(3) * quat(3));
            dF(4, 2) = 2 * (mag(1) * quat(2) + mag(2) * quat(3) + mag(3) * quat(4));
            dF(4, 3) = 2 * (-mag(1) * quat(3) + mag(2) * quat(2) + mag(3) * quat(1));
            dF(4, 4) = 2 * (-mag(1) * quat(4) - mag(2) * quat(1) + mag(3) * quat(2));
            F(1) = -(quat(2) * quat(4) - quat(1) * quat(3) + acc(1) / (2 * g));
            F(2) = -(quat(3) * quat(4) + quat(1) * quat(2) + acc(2) / (2 * g));
            F(3) = -(quat(1) * quat(1) - quat(2) * quat(2) - quat(3) * quat(3) + quat(4) * quat(4) + acc(3) / g);
            F(4) = -(mag(1) * (quat(1) * quat(1) + quat(2) * quat(2) - quat(3) * quat(3) - quat(4) * quat(4))
                + 2 * mag(2) * (quat(2) * quat(3) - quat(1) * quat(4)) + 2 * mag(3) * (quat(2) * quat(4) + quat(1) * quat(3)));
            LinearEquationSolve(dF, F, dquat);
            if (dquat.norm() > 1)
                dquat.normalize();
            quat += dquat;
            quat.normalize();
        } while (curStep <= maxStep && dquat.norm() > error);
        std::cout << "*************************************" << std::endl;
        std::cout << "step : " << curStep << std::endl;
        std::cout << "result : " << std::endl;
        std::cout << quat << std::endl;
        cEulerAngle ans(quat);
        std::cout << ans*57.3 << std::endl;
    }
    return curStep;
}
 
LinearEquationSolve()解线性方程组采用全主元Doolittle方法。
 

在平常的开发工作中，时常会用到第三方SDK，一般都需要在Application中初始话，随着集成的SDK后，Application类就可能会变成这样：
 
class MainApplication : Application() 
{
    override fun onCreate()
    {
        super.onCreate()
        ARouter.init(this)
        UMConfigure.init(this,...)
        Realm.init(this)
        ToastUtils.init(this)
    }
}
 
所有的SDK初始化都放在这里会拖慢App的启动速度，所以需要优化；当然我们可以开线程去初始化，可以在用得到的时候再初始化，这里我们介绍一种新的集成方式（参照FireBase实现）：
 
1：把我们的SDK全部放进一个新的Module中并依赖（注意依赖的时候使用api方式，否则主模块将无法使用SDK）：
 
 
2：新建InitSDKProvider实现ContentProvider并实现接口方法，并且在onCreate函数中进行所依赖SDK的初始化：
 
 
3：在Androidmanifgest.xml中添加provider节点：
 
 
最后，主模块依赖AARProviderTest模块，并在代码中调用方法进行使用。
 
工程代码链接：https://github.com/chen8888/AARProviderTest