我在用卡尔曼估计一个非线性方程的参数上遇到了问题

。希望大家帮我度过难关。具体的问题是,已知非线性方程的输入输出,

我要估计非线性方程的六个参数,即X这个状态,矩阵(1x6),把非线性方程的输出信号组成Z(1xn列),共n个数,所以整个卡尔曼递推从2-n,

我把非线性方程又把非线性方程输入信号组成了H这个观测矩阵(1维n列时变矩阵)

然后运用卡尔曼算法递推

但是我出现了问题。
卡尔曼的噪声设置
R=1e-7*var(y);
Q=1;%
运行结果及报错内容
状态中的第一个参数,在递推2-n过程中,不收敛

我的解答思路和尝试过的方法
我想要达到的结果