卡尔曼滤波(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不认为是方差为零的理想数据。
**********************************************************************************************************************/

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

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

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

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

以下代码角度测量为例，误差维度为2，即角度和角速度
typedef struct{
float x[2]; //初始状态值矩阵，角度和角速度
float p[2][2]; //初始方差矩阵，角度方差和角速度方差
float A[2][2]; //初始状态矩阵系数，自状态变化系数一般A11与A22为1，相关系数A12，A21系统决定
float H[2]; //测量系数矩阵，角度测量和角速度测量
float q[2]; //过程噪声矩阵，角度过程噪声和角速度过程噪声
float r; //测量噪声矩阵，角度测量噪声，它是一维是因为实际应用中只有测量角速度，而角度是通过角速度计算而来的
float gain[2]; //卡尔曼增益，角度增益和角速度增益
}kalman2_state;
/*********************************************************************************************************
* @brief
* Init fields of structure @kalman1_state.
* I make some defaults in this init function:相关系数A12，A21
* A = {{1, 0.1}, {0, 1}};
* H = {1,0};
* and @q,@r are valued after prior tests.
*
* NOTES: Please change A,H,q,r according to your application.
*
* @inputs
* @outputs
* @retval
*/
void kalman2_init(kalman2_state *state, float *init_x, float (*init_p)[2])
{
state->x[0] = init_x[0]; //初始角度状态值，可为0
state->x[1] = init_x[1]; //初始角速度状态值，可为0
state->p[0][0] = init_p[0][0]; ////初始角度自变化方差，取小值
state->p[0][1] = init_p[0][1]; ////初始角速度影响角度的方差变化，取小值
state->p[1][0] = init_p[1][0]; ////初始角度影响角速度的方差变化，取小值
state->p[1][1] = init_p[1][1]; ////初始角速度自变化方差，取小值
//state->A = {{1, 0.1}, {0, 1}}; //
state->A[0][0] = 1; //角度自状态变化系数为1
state->A[0][1] = 0.1; //角速度自状态变化系数为1
state->A[1][0] = 0; //角速度影响角度状态变化系数为0.1
state->A[1][1] = 1; //角度影响角速度状态变化系数为0
//state->H = {1,0}; //
state->H[0] = 1; //角度的测量系数
state->H[1] = 0; //角速度的测量系数
//state->q = {{10e-6,0}, {0,10e-6}}; /* measure noise convariance */
state->q[0] = 10e-7; //
state->q[1] = 10e-7; //
state->r = 10e-7; /* estimated error convariance */
}

/*********************************************************************************************************
* @brief
* 2 Dimension kalman filter
* @inputs
* state - Klaman filter structure
* z_measure - Measure value
* @outputs
* state->x[0] - Updated state value, Such as angle,velocity
* state->x[1] - Updated state value, Such as diffrence angle, acceleration

* state->p - Updated estimated error convatiance matrix
* @retval
* Return value is equals to state->x[0], so maybe angle or

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

velocity.
*/
float kalman2_filter(kalman2_state *state, float angle,float gyro)
{
float temp0;
float temp1;
float temp;

/* Step1: Predict */
state->x[0] = state->A[0][0] * state->x[0] + state->A[0][1] * state->x[1]; //预测当前角速度状态值 自变化+互变化 由于state->A[0][1]为0.1，所以角度的变化是由角速度的测量*0.1得到角度，现有的角度，此式子即是角速度积分累加成角度
state->x[1] = state->A[1][0] * state->x[0] + state->A[1][1] * state->x[1];//预测当前角度状态值 自变化+互变化 由于state->A[1][0]为0，所以角速度的变化是由角速度的测量自己决定
/* p(n|n-1)=A^2*p(n-1|n-1)+q */
state->p[0][0] = state->A[0][0] * state->p[0][0] + state->A[0][1] * state->p[1][0] + state->q[0]; //计算角度方差P，实际上是P(k|k-1)=A*P(k-1|k-1) +Q
state->p[0][1] = state->A[0][0] * state->p[0][1] + state->A[1][1] * state->p[1][1]; //
state->p[1][0] = state->A[1][0] * state->p[0][0] + state->A[0][1] * state->p[1][0]; //
state->p[1][1] = state->A[1][0] * state->p[0][1] + state->A[1][1] * state->p[1][1] + state->q[1]; //计算角速度方差P，实际上是P(k|k-1)=A*P(k-1|k-1) +Q

/* Step2: Measurement */
/* gain = p * H^T * [r + H * p * H^T]^(-1), H^T means transpose. */
temp0 = state->p[0][0] * state->H[0] + state->p[0][1] * state->H[1]; // 角度中间变量p * H^T
temp1 = state->p[1][0] * state->H[0] + state->p[1][1] * state->H[1]; // 角速度中间变量p * H^T
temp = state->r + state->H[0] * temp0 + state->H[1] * temp1; //卡尔曼增益计算公式的分母，测量噪声在此融合r + H * p * H^T]^(-1)，因为角度是计算出来的，角速度是测量出来的，唯一测量噪声只有一个
state->gain[0] = temp0 / temp; //计算角度的卡尔曼增益
state->gain[1] = temp1 / temp; //计算角速度的卡尔曼增益
/* x(n|n) = x(n|n-1) + gain(n) * [z_measure - H(n)*x(n|n-1)]*/
temp = state->H[0] * state->x[0] + state->H[1] * state->x[1]; //更新当前状态，卡尔曼滤波器也在此输入
state->x[0] = state->x[0] + state->gain[0] *(angle - temp);
state->x[1] = state->x[1] + state->gain[1] * (gyro -temp);

/* Update @p: p(n|n) = [I - gain * H] * p(n|n-1) */
state->p[0][0] = (1 - state->gain[0] * state->H[0]) * state->p[0][0]; //更新协方差
state->p[0][1] = (1 - state->gain[0] * state->H[1]) * state->p[0][1];
state->p[1][0] = (1 - state->gain[1] * state->H[0]
) * state->p[1][0];
state->p[1][1] = (1 - state->gain[1] * state->H[1]) * state->p[1][1];

return state->x[0]; //卡尔曼输出

本文来源：https://www.bwwdw.com/article/ax1j.html