二维卡尔曼滤波应用

最新推荐文章于 2024-04-29 11:09:15 发布

wang_22ok

最新推荐文章于 2024-04-29 11:09:15 发布

阅读量5.8k

点赞数 8

分类专栏：四旋翼飞行器文章标签：卡尔曼滤波

本文链接：https://blog.csdn.net/wang_22ok/article/details/84973614

版权

四旋翼飞行器专栏收录该内容

1 篇文章 0 订阅

订阅专栏

关于卡尔曼滤波的理解@wang_22ok

二维卡尔曼滤波应用

卡尔曼滤波的五大黄金公式如下：
在这里插入图片描述

关于位置与速度的卡尔曼滤波

首先位置和速度的关系如下： $\begin{array}{l} x(t) = x(t - 1) + x(t - 1)\Delta T + \frac{1}{2}a{(\Delta T)^2}\\ v(t) = v(t - 1) + a\Delta T \end{array}\tag1$
上式两个表达式写成矩阵的形式如下：
$\left[ \begin{matrix} x(t)\\ v(t) \end{matrix} \right] = \left[ {\begin{matrix} 1&{\Delta T}\\ 0&1 \end{matrix}} \right]\left[ \begin{array}{l} x(t - 1)\\ v(t - 1) \end{array} \right] + \left[ \begin{array}{l} \frac{{{{(\Delta T)}^2}}}{2}\\ \Delta T \end{array} \right]a\tag2$
其中， $a$ 为加速度。

由卡尔曼滤波的算法流程：

状态向量预处理方程：
$X_k^{ - } = AX_{k-1} + BU_k\tag3$
由公式(2)，有： $\left[ {\begin{matrix} 1&{\Delta T}\\ 0&1 \end{matrix}} \right],B = \left[ \begin{matrix}{l} \frac{{{{(\Delta T)}^2}}}{2}\\ \Delta T \end{matrix} \right]$
简化： $\begin{array}{l} x(t) = x(t - 1) + v(t - 1)*\Delta T + \frac{{{{(\Delta T)}^2}}}{2}*a;\\ v(t) = v(t - 1) + a*\Delta T; \end{array}$
状态向量计算预处理协方差矩阵：
$P_k^{ - } = AP_{k-1}{A^T} + Q\tag4$
有：
${P_k^{ - }} = \left[ {\begin{matrix} {{P_{k1}^{ - }}}&{{P_{k2}^{ - }}}\\ {{P_{k3}^{ - }}}&{{P_{k4}^{ - }}} \end{matrix}} \right] = \left[ {\begin{matrix} 1&{\Delta T}\\ 0&1 \end{matrix}} \right]*\left[ {\begin{matrix} {{P_{(k - 1)1}}}&{{P_{(k - 1)2}}}\\ {{P_{(k - 1)3}}}&{{P_{(k - 1)4}}} \end{matrix}} \right]*\left[ {\begin{matrix} 1&0\\ {\Delta T}&1 \end{matrix}} \right] + \left[ {\begin{matrix} {{Q_1}}&{{Q_2}}\\ {{Q_3}}&{{Q_4}} \end{matrix}} \right]$
简化： $\begin{array}{l} {P_{k1}^{ - }} = ({P_{(k - 1)1}} + {P_{(k - 1)3}}*\Delta T) + ({P_{(k - 1)2}} + {P_{(k - 1)4}}*\Delta T)*\Delta T + {Q_1};\\ {P_{k2}^{ - }} = {P_{(k - 1)2}} + {P_{(k - 1)4}}*\Delta T + {Q_2};\\ {P_{k3}^{ - }} = {P_{(k - 1)3}} + {P_{(k - 1)4}}*\Delta T + {Q_2};\\ {P_{k4}^{ - }} = {P_{(k - 1)4}} + {Q_4}; \end{array}$
Kalman增益矩阵：
${K_k} = P_k^{ - }H^T{({H}P_k^{ - 1}H^T + {R_k})^{ - 1}}\tag5$
状态向量更新方程：
${X_k} = X_k^ - + {K_k}({Z_k} - {H}X_k^ - )\tag6$
状态向量协方差更新方程：
${P_k} = (I - {K_k}{H})P_k^ - \tag7$

程序如下：

typedef struct {
      float x[2];     /* state: [0]-angle [1]-diffrence of angle, 2x1 */
      float A[2][2];  /* X(n)=A*X(n-1)+U(n),U(n)~N(0,q), 2x2 */
      float H[2][2];     /* Z(n)=H*X(n)+W(n),W(n)~N(0,r), 1x2   */
      float q[2];     /* process(predict) noise convariance,2x1 [q0,0; 0,q1] */
      float r[2][2];        /* measure noise convariance */
      float p[2][2];  /* estimated error convariance,2x2 [p0 p1; p2 p3] */
      float gain[2][2];  /* 2x1 */
	  float B[2];
} kalman2_state;       


void kalman2_init(kalman2_state_Wang *state)//, float *init_x, float (*init_p)[2]//×îºóÖ»Ðèµ÷ÊÔq[0],q[1];
{
//    state->x[0]    = init_x[0];
//    state->x[1]    = init_x[1];
//    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->x[0]    = 0;
	  state->x[1]    = 0;
      state->p[0][0] = 1;
      state->p[0][1] = 0;
      state->p[1][0] = 0;
      state->p[1][1] = 1;
//	  state->A       = {{1, 0.1}, {0, 1}};
      state->A[0][0] = 1;
      state->A[0][1] = 0.01;//1;//t¿É±ä  Á½¸öÎïÀíÊ±¿ÌµÄ²î2ms      PID_PIT.I*0.27;//0.0027
      state->A[1][0] = 0;
      state->A[1][1] = 1;
//	  state->H       = {1,0};
      state->H[0][0] = 1;
      state->H[0][1] = 0;//1
	  state->H[1][0] = 0;
	  state->H[1][1] = 0;
//    state->q       = {{10e-6,0}, {0,10e-6}};  /* measure noise convariance */		
	  state->q[0]    = 5*10e-8;//5;//0.0001;//10e-7;//10e-7;
      state->q[1]    = 5*10e-8;//10e-6;//0.5;//0.0035;//5*10e-7;
      state->r[0][0] = 1*10e-7;//10e-4;//52.4586;//0.1;//10e-3;//10e-7;  /* estimated error convariance */PID_ROL.D*
	  state->r[0][1] = 0;
	  state->r[1][0] = 0;
	  state->r[1][1] = 4*10e-4;
//    state->B		
	  state->B[0]    = state->A[0][1]* state->A[0][1]/2.0;
	  state->B[1]    = state->A[0][1];
}

float kalman2_filter(kalman2_state_Wang *state, float x_weiyi,float x_speed,float a)//£¨¶þÎ¬¿¨¶ûÂüÂË²¨£©Ö»±äÒ»¸ö£¬Ðè¸Ä½øÎ»ÒÆ¡¢ËÙ¶È,ÕæÕýµÄ¼ÓËÙ¶È£¨ÐÞ¸Äºó£©
{
		float temp0 = 0.0f;
		float temp1 = 0.0f;
		float temp0_0 = 0.0f;
		float temp0_1 = 0.0f;
		float temp1_0 = 0.0f;
		float temp1_1 = 0.0f;
		float temp00 = 0.0f;
		float temp01 = 0.0f;
		float temp10 = 0.0f;
		float temp11 = 0.0f;

    /* Step1: Predict X(k+1)= A*X(k) +B*U(k)*/
		state->x[0] = state->A[0][0] * state->x[0] + state->A[0][1] * state->x[1]+state->B[0]*a;//×ª»»Íê×ø±ê·½¿ÉÊ¹ÓÃ
		state->x[1] = state->A[1][0] * state->x[0] + state->A[1][1] * state->x[1]+state->B[1]*a;
    /* Step2: Covariance Predict P(k+1)=A*P(k)*(A^T)+Q;*/
		state->p[0][0] = (state->p[0][0]+state->p[1][0]*state->A[0][1])+(state->p[0][1]+state->p[1][1]*state->A[0][1])*state->A[0][1]+state->q[0];
		state->p[0][1] = state->p[0][1]+state->p[1][1]*state->A[0][1];//+state->q[0];
		state->p[1][0] = state->p[1][0]+state->p[1][1]*state->A[0][1];//+state->q[1];
		state->p[1][1] = state->p[1][1]+state->q[1];
    /* Step3: Gain Measurement : gain = p * H^T * [r + H * p * H^T]^(-1), H^T means transpose.  µÚÈý¸ö¹«Ê½×ª»»*/
		temp0_0  = (state->p[0][0] +state->r[0][0])*(state->p[1][1] +state->r[1][1])-(state->p[0][1] +state->r[0][1])*(state->p[1][0] +state->r[1][0]);//ÕýÈ·//r¶Ô½ÇÕó
		temp0_1  = (state->p[0][0] +state->r[0][0])*(state->p[1][1] +state->r[1][1])-(state->p[0][1] +state->r[0][1])*(state->p[1][0] +state->r[1][0]);
		temp1_0  = (state->p[0][0] +state->r[0][0])*(state->p[1][1] +state->r[1][1])-(state->p[0][1] +state->r[0][1])*(state->p[1][0] +state->r[1][0]);
		temp1_1  = (state->p[0][0] +state->r[0][0])*(state->p[1][1] +state->r[1][1])-(state->p[0][1] +state->r[0][1])*(state->p[1][0] +state->r[1][0]);		
		temp00  =   state->p[1][1] / temp0_0;
		temp01  =  -state->p[0][1] / temp0_1;
		temp10  =  -state->p[1][0] / temp1_0;
		temp11  =   state->p[0][0] / temp1_1;	
		state->gain[0][0] = state->p[0][0]* temp00 + state->p[0][1]* temp10;
		state->gain[0][1] = state->p[0][0]* temp01 + state->p[0][1]* temp11;
		state->gain[1][0] = state->p[1][0]* temp00 + state->p[1][1]* temp10;
		state->gain[1][1] = state->p[1][0]* temp01 + state->p[1][1]* temp11;
    /* Step4: Status Update : x(n|n) = x(n|n-1) + gain(n) * [z_measure - H(n)*x(n|n-1)]*/
		state->x[0] = state->x[0] + state->gain[0][0] * (x_weiyi - state->x[0])+ state->gain[0][1] * (x_speed - state->x[1]); //ÎªºÎÖ»ÓÃÒ»¸ö²ÎÊý
		state->x[1] = state->x[1] + state->gain[1][0] * (x_weiyi - state->x[0])+ state->gain[1][1] * (x_speed - state->x[1]); 
    /* Step5: Covariance Update p: p(n|n) = [I - gain * H] * p(n|n-1)  ¸üÐÂp*/
		temp0=state->p[0][0];
		temp1=state->p[0][1];
		state->p[0][0] = (1 - state->gain[0][0] ) * state->p[0][0]-(state->gain[0][1]* state->p[1][0]);
		state->p[0][1] = (1 - state->gain[0][0] ) * state->p[0][1]-(state->gain[0][1]* state->p[1][1]);
		state->p[1][0] = (1 - state->gain[1][1] ) * state->p[1][0]-state->gain[1][0]* temp0;//state->p[0][0]
		state->p[1][1] = (1 - state->gain[1][1] ) * state->p[1][1]-state->gain[1][0]* temp1;//state->p[0][1]

    return 1;
}


//z轴kalman滤波初始化，初始化时用
kalman2_init(&BaroAlt_klm);
//输入气压计高度，速度和惯性坐标下的加速度---------输出高度和速度
kalman2_filter(&BaroAlt_klm, BaroAltoo,0,az_c);