卡尔曼滤波器针对的问题通常可以使用两个方程来描述:
对于一个随机离散时间过程的状态向量
其中下标k和k-1分别表示对应的时刻。x是系统状态变量,A是状态转移矩阵或者过程增益矩阵,u是控制输入,B是对应的控制输入增益,w是过程激励噪声,认为是高斯白噪声,期望是0,协方差是Q。
观测方程
其中z是观测值,v是观测噪声,认为是高斯白噪声,期望是0,协方差是R。
对于以上方程描述的问题,卡尔曼滤波器主要通过5个公式来求得。下面结合陀螺仪加速度计求解欧拉角的问题对公式进行说明。
时间更新方程(预估方程):
第一个方程:
在本案例中,状态变量是角度和陀螺仪获得的角速度,并且没有其他控制输入变量,因此以上方程可以写成
第二个方程:
对于上述状态变量,协方差矩阵的含义为
为了书写方便,记为
过程激励噪声矩阵为
在本案例中,
对以上方程简化:
需要注意以上方程右侧第二个矩阵第一项省略二阶无穷小。
测量更新方程(校正方程):
第三个方程:
在本案例下,观测变量是加速度计获得的角度
其中
代入可以得到:
第4个方程:
在本案例下,代入可以得到:
第5个方程:
在本案例下,代入可以得到:
以上推导过程和如下博文矛盾:
https://blog.csdn.net/dyl74500196/article/details/59108807
区别在于:我认为状态变量应当是角度和陀螺仪获得的角加速度,而博客上认为状态变量应当是角度和陀螺仪的漂移。究竟哪种说法是正确的?