Quadratic cost of MPC given the sequence of states, inputs and the weight matrices Q, R and Pf.

gistfile1.c

zreal primal_cost(const cs *Q, const cs *R, const cs *Pf, const ptrdiff_t N, const zmatr* X, const zmatr* U) {
    zreal J = 0;
    ptrdiff_t k;
    zvect* xk = malloc(sizeof(zvect));
    zvect* uk = malloc(sizeof(zvect));
    for (k = 0; k < N; k++) {
        *xk = X->values + k;
        *uk = U->values + k;
        J += dvsm_quad(Q, *xk);
        J += dvsm_quad(R, *uk);
    }
    J += dvsm_quad(Pf, X->values + (N - 1));
    return J;
}

gistfile2.c

/* 
 * File:   gpad.h
 * Author: Pantelis Sopasakis
 *
 * Created on January 25, 2013, 5:30 PM
 */

#ifndef GPAD_H
#define    GPAD_H

#include "cs.h"


#ifdef	__cplusplus
extern "C" {
#endif

    typedef double zreal;

    typedef zreal* zvect;

    /**
     * An nrows-by-ncols (dense) matrix. This structure holds the values of 
     * the matrix as well as its dimensions.
     */
    typedef struct {
        int nrows;
        int ncols;
        zvect values;
    } zmatr;

    /**
     * Dense Vector - Sparse Matrix Quadratic Form.
     * @param Q
     *      A sparse square matrix.
     * @param x
     *      A vector.
     * @return 
     *      Returns the quadratic form x'Qx
     */
    double dvsm_quad(const cs* Q, const zvect x);

    /**
     * Dense to Sparse.
     * @param Xsp
     *          Sparse matrix to be updated.
     * @param X
     *          Dense matrix to be converted.
     */
    void dts(cs* Xsp, const zmatr* X);
    
    /**
     * 
     * @param Q
     * @param R
     * @param Pf
     * @param N
     *          The prediction horizon.
     * @param X
     *          Sequence of states in column-major order as a pointer zmatr.
     * @param U
     *          Sequence of inputs in column-major order.
     * @return 
     *          The value of the primal cost as a zreal.
     */
    zreal primal_cost(const cs *Q, const cs *R, const cs *Pf, const ptrdiff_t N, const zmatr* X, const zmatr* U);


#ifdef	__cplusplus
}
#endif

#endif	/* GPAD_H */