多元卡尔曼滤波器中的R分配数组

Question

我可以创建一个多元Kalman模型，如下所示。 但是，我不确定是否已正确分配阵列。

static void Stuff(Speed speed, Color sn_color, Color *color, char *name, char *direction)
{
    if (name)
    {
        set_sensor_mode(sn_color, "COL-COLOR");
        get_sensor_value(0, sn_color, &color);
        printf("%s_color is %d\n", name, color);
        if (color == 3)
        {
            printf("turn %s\n", direction);
            killMotors(0);
            return 0;
        }
    }
    pidLine(speed);
}

…
    if (rx_color <= 60)
        Stuff(speed, sn_rx_color, &rx_color, "rx", "right")
    else if (lx_color <= 60)
        Stuff(speed, sn_lx_color, &lx_color, "lx", "left")
    else
        Stuff(speed, 0, 0, 0, 0);

此外，结果似乎与我分别运行每个单变量模型的结果相同。 这可能是因为我未正确设置数组。

我如何运行包含协变量的模型，以使它们具有相互依赖性，并且每个因素都起作用，从而使结果对单个单变量模型有所改善？

谢谢您的帮助。