求一段卡尔曼代码，
我写了一个是下面的代码算不算卡尔曼滤波？输入Z1为加速度，
为什么kg(k)和p(k|k)到最后为一个固定值？
对矩阵不懂，只想输入一个信号然后滤波，求解答
void kalman(short int z1)
{
x（k|k-1） = x(k-1|k-1);
p(k|k-1) = sqrt( p(k-1|k-1)* p(k-1|k-1) + 16);
kg(k) = sqrt ( p(k|k-1) * p(k|k-1) /( p(k|k-1) * p(k|k-1)+16 ) );
x(k|k)=x(k|K-1)+( kg(k) * (z1 - x(k|k) ) );
p(k|k)=sqrt( ( 1-kg(k|k)) *p(k|k-1)* p(k|k-1) );
xin(k-1|k-1)=xin(k|k);
p(k-1|k-1)=p(k|K);
}
|
楼主
标题置顶
标题高亮
