卡尔曼滤波(C语言,二维)

卡尔曼滤波,C语言,二维

/*******************************************************************
*@title kalman filter;
*@brief 一维卡尔曼滤波器
*@time 2016.10.19
*@editor小南&zin
*飞控爱好QQ群551883670,邮箱759421287@
******************************************************************/
#include "kalman.h"
#include <math.h>
#include "sys.h"
/*********************************************************************************************************************
**卡尔曼滤波
**@brief: 线性最优评估滤波
**@param[in] InputData 滤波前的数据，QR误差
**@param[out] None
**@return 滤波后的数据
**@remark: 通过修改过程噪声和测量噪声R,Q值优化输出值
X(k)=A X(k-1)+B U(k)+W(k)
Z(k)=H X(k)+V(k)
AB是系统参数
XK时刻的系统状态
H：测量系统的参数
Z：K时刻的测量值
WV：过程和测量噪声

X(k|k-1)=X(k-1|k-1) 预测下一状态的系统
P(k|k-1)=P(k-1|k-1) +Q
Kg(k)= P(k|k-1) / (P(k|k-1) + R) 计算Kg卡尔曼增益
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1)) 根据预测值结合估算值得出当前状态值
P(k|k)=(1-Kg(k))P(k|k-1) 更新协方差


(k-1|k-1)上一个状态的最优评估
(k|k-1) 由上一个状态的最优评估预测当前状态的最优评估
(k|k) 由预测本状态的评估具体实现最优评估

Q:系统过程的协方差
R：测量的协方差
高斯白 = Q+R
Kg：卡尔曼增益
P：协方差


强调：调整卡尔曼的滤波效果时只要调整过程噪声和测量噪声即可，初始X可0，P取小值
强调：调整卡尔曼的滤波效果时只要调整过程噪声和测量噪声即可，初始X可0，P取小值
强调：调整卡尔曼的滤波效果时只要调整过程噪声和测量噪声即可，初始X可0，P取小值
强调：调整卡尔曼的滤波效果时只要调整过程噪声和测量噪声即可，初始X可0，P取小值
因为X是初始状态，也就是进入测量前的数据，任意设定。初始时候还未进入测量的时候的数值X不认为是方差为零的理想数据。
**********************************************************************************************************************/

//调整卡尔曼的滤波效果时只要调整过程噪声和测量噪声即可

//卡尔曼滤波属于专门针对高斯白的滤波器，无法应对突变干扰，所以加一个除去最大最小值的滑动均值滤波。

//多维卡尔曼是多维噪声的扩展，如在飞行器的系统中，一维是加速度的误差，二维是加速度和角速度两个维度的误差，
//三维则是角速度，加速度，磁力轴的三个维度误差。
//矩阵的维数与卡尔曼的维数相同
/********************************************************************************************
************/
//卡尔曼的函数形式，可在外部调用
/********************************************************************************************************/
//