2015.6.24—卡尔曼滤波（一维）程序与注释

6-24 算法 jevy 2,304PVs

2015.6.24—卡尔曼滤波（一维）程序与注释

6-24 2,304PVs

float kalman1_filter(kalman1_state *state, float z_measure)
{
    /* Predict 预测
    
	状态方程： 
	X(k)=A*X(k-1)+B*U(k)+W(k) 
	
	测量方程： 
	Z(k)=H*X(k)+V(k) 
	
	预测下一状态:
	x(k/k-1)=A*X(k-1|k-1)+BU(k)，这里U(k)为0。 （1） 
    
	p表示covariances，误差协方差，当卡尔曼滤波稳定后，应该在一定范围内 
    P(k|k-1)=A*P(k-1|k-1)A' + Q    （2） 
    
    通过（1）式可以预测出现在的系统状态，是估计值。然后接下来需要收集现在状态的
	测量值，结合预测值和测量值，可以得到现在状态k的最优化估算值x(k|k)：
	x(k|k) = A*x(k|k-1)+Kg(k)(Z(k)-H*A*x(k|k-1))   (3) 
	
	Kg(k)为卡尔曼增益(Kalman Gain):
	Kg(k) = P(k|k-1)H'/(H*P(k|k-1)H' + R)     (4) 
	
	我们的得到了k状态下的最优估计值x(k|k)。但是要让卡尔曼滤波器不断运行，
	我们还要更新k状态下x(k|k)的covariance:
	P(k|k)=(I-Kg(k)*H)*P(k|k-1)       (5) 
	*/
    state->x = state->A * state->x;
    state->p = state->A * state->A * state->p + state->q;  /* p(n|n-1)=A^2*p(n-1|n-1)+q */

    /* Measurement */
    state->gain = state->p * state->H / (state->p * state->H * state->H + state->r);
    state->x = state->x + state->gain * (z_measure - state->H * state->x);
    state->p = (1 - state->gain * state->H) * state->p;

    return state->x;
}

float kalman1_filter(kalman1_state *state, float z_measure)

{

/* Predict 预测

状态方程：

X(k)=A*X(k-1)+B*U(k)+W(k)

测量方程：

Z(k)=H*X(k)+V(k)

预测下一状态:

x(k/k-1)=A*X(k-1|k-1)+BU(k)，这里U(k)为0。（1）

p表示covariances，误差协方差，当卡尔曼滤波稳定后，应该在一定范围内

P(k|k-1)=A*P(k-1|k-1)A' + Q （2）

通过（1）式可以预测出现在的系统状态，是估计值。然后接下来需要收集现在状态的

测量值，结合预测值和测量值，可以得到现在状态k的最优化估算值x(k|k)：

x(k|k) = A*x(k|k-1)+Kg(k)(Z(k)-H*A*x(k|k-1)) (3)

Kg(k)为卡尔曼增益(Kalman Gain):

Kg(k) = P(k|k-1)H'/(H*P(k|k-1)H' + R) (4)

我们的得到了k状态下的最优估计值x(k|k)。但是要让卡尔曼滤波器不断运行，

我们还要更新k状态下x(k|k)的covariance:

P(k|k)=(I-Kg(k)*H)*P(k|k-1) (5)

state->x = state->A * state->x;

state->p = state->A * state->A * state->p + state->q; /* p(n|n-1)=A^2*p(n-1|n-1)+q */

/* Measurement */

state->gain = state->p * state->H / (state->p * state->H * state->H + state->r);

state->x = state->x + state->gain * (z_measure - state->H * state->x);

state->p = (1 - state->gain * state->H) * state->p;

return state->x;

}

你没有看错，这么牛的算法，用C语言只有几行。

版权属于: JevyLee's Blog | 记录经验，分享知识，表达价值

原文地址: http://www.jevylee.com/?p=1345

转载时必须以链接形式注明原始出处及本声明。

2015.6.24—卡尔曼滤波（一维）程序与注释

2015.6.24—卡尔曼滤波（一维）程序与注释

2 条评论

欢迎留言取消回复

2015.6.24—卡尔曼滤波（一维）程序与注释

2015.6.24—卡尔曼滤波（一维）程序与注释

2 条评论

欢迎留言 取消回复

欢迎留言取消回复