TN07-卡尔曼滤波在姿态解算和位置估计中的应用.docx

TN07–卡尔曼滤波在姿态解算和位置估计中的应用卡尔曼滤波参考：ttp://zh.wikipedia.org/wiki/卡尔曼滤波对象模型（）（）其中为状态量，为观测量，和分别为过程噪声和输出噪声，假定其为均值为0，协方差矩阵为和的正态分布噪声。滤波算法预测(状态预估)（）(状态协方差预估)（）误差（测量误差）（）（测量误差协方差）（）（最优卡尔曼增益）（）更新（状态更新）（）（状态协方差更新）（）其中协方差准确的反映了估计的协方差扩展卡尔曼滤波扩展卡尔曼滤波应用卡尔曼滤波的算法于非线性对象，对象模型为（）（）其中变量的意义与卡尔曼滤波对象模型相同。在卡尔曼滤波算法中，分别用非线性函数估计对象的状态和观测量，用非线性函数的Joccob矩阵进行状态和协方差的更新运算，如下预测(状态预估)（）(状态协方差预估)（）其中(状态转换矩阵)（）误差（测量误差）（）（测量误差协方差）（）（最优卡尔曼增益）（）其中(输出矩阵)（）更新（状态更新）（）（状态协方差更新）（）姿态解算姿态更新模型（）其中（），为姿态的四元素表示。实际应用中，角速度为可测量，另外加速度和指南针为可观测量。建立离散形式为（）（）（）其中（）如果采用精细微分法，则有（）和为地面坐标系下的加速度和磁场矢量，为地面坐标系到机体坐标系的转换矩阵（）计算偏导求取输出Jaccob矩阵（）（）位置估计对象模型（）（）其中和分别为过程噪声和观测噪声