matlab滤波后nan,用一阶滤波和卡尔曼滤波解算姿态角时结果为nan

尝试用arduino和simulink制作两轮自平衡小车。      现在可以读到MPU6050的六轴原始数据了，想用MATLAB Function解算出俯仰角。由于无论是一阶滤波方法还是卡尔曼滤波方法都是迭代出结果的，需要用到全局变量。但是输出的结果始终是nan。

求助如何解决？

下面是程序：

一阶滤波：

function y = fcn(acc, gyro)

%#codegen

global angle1;

K1 =0.1;

dt = 0.005;

angle_m = atan(acc(1)/acc(3))*57.3;

gyro_m = -gyro(2)/16.4;

angle1 = K1 * angle_m + (1-K1) * (angle1 + gyro_m * dt);

y = angle1;

卡尔曼滤波：

function [y, angle_dot] = fcn(acc, gyro)

%#codegen

global anglek;

global q_bias;

global P;

global Pdot;

Q_angle=0.001;

Q_gyro=0.003;

C_0 = 1;

R_angle=0.5;

dt=0.005;

angle_m = atan(acc(1)/acc(3))*57.3;

gyro_m = -gyro(2)/16.4;

anglek = anglek + (gyro_m - q_bias) * dt;

angle_err = angle_m - anglek;

Pdot(1)=Q_angle - P(1,2) - P(2,1);

Pdot(2) = -P(2,2);

Pdot(3) = -P(2,2);

Pdot(4)= Q_gyro;

P(1,1) = P(1,1)+Pdot(1) * dt;

P(1,2) = P(1,2)+Pdot(2) * dt;

P(2,1) = P(2,1)+Pdot(3) * dt;

P(2,2) = P(2,2)+Pdot(4) * dt;

PCt_0 = C_0 * P(1,1);

PCt_1 = C_0 * P(2,1);

E = R_angle + C_0 * PCt_0;

K_0 = PCt_0 / E;

K_1 = PCt_1 / E;

t_0 = PCt_0;

t_1 = C_0 * P(1,2);

P(1,1) = P(1,1)-K_0 * t_0;

P(1,2) = P(1,2)-K_0 * t_1;

P(2,1) = P(2,1)-K_1 * t_0;

P(2,2) = P(2,2)-K_1 * t_1;

anglek = anglek+K_0 * angle_err; %最优角度

q_bias = q_bias+K_1 * angle_err;

angle_dot = gyro_m-q_bias; %最优角速度

y = anglek;

我认为程序原理应该是没问题的，因为网上找到的程序都大同小异，可是运行结果始终是nan，想不明白。

2018-11-28 13:38 上传

simulink框图